
//#include "test6.h"
#include "ros/ros.h"
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/LaserScan.h>
#include <visualization_msgs/Marker.h>
#include <nav_msgs/Odometry.h>

#include <vector>
#include <set>
//#include <multiset>

#include <unordered_map>
#include <tuple>
#include <cmath>
#include <iostream>
#include <queue>
#include <map>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>

#include <stdexcept> // 使用标准异常处理
#include <condition_variable>
#include "fanguangbantrilateration/trilateration_srvs.h"
#include "fanguangbantrilateration/trilateration_read_time_log.h"
#include "fanguangbantrilateration/matchcostmapwithscan_srvs.h"
#include "fanguangbantrilateration/matchcostmapwithscan_result.h"


#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
//参数服务 trilaterationParam.cfg trilateration 对应 PACKAGE = 'trilateration'   ；     exit(gen.generate(PACKAGE, "trilateration", "TrilaterationParam")) 中的  "TrilaterationParam" 对应 TrilaterationParamConfig.h
#include <dynamic_reconfigure/server.h>
#include "fanguangbantrilateration/TrilaterationParamConfig.h"

#include "nav_msgs/Path.h"
#include "geometry_msgs/PoseStamped.h"

#include "geometry_msgs/Pose2D.h"

//uuid
#include <chrono>
#include <random>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <thread>
#include <atomic>
#include <mutex>
//#include <algorithm> // 添加这一行以包含 std::sort
#include <memory>   //std::shared_ptr

#include "json.h"
#include "parser.h"
#include<signal.h>

//直接优化法 使用 Ceres Solver 来进行优化
#include <ceres/ceres.h>
#include <ceres/rotation.h>


#include <deque>



/**
 * 反光柱定位算法主主要节点
 * 此版本是针对长走廊环境设计的一款反光柱定位产品。
 * 在窄长走廊中，由于空间限制只能一维部署反光柱，难以具备唯一特征。
 * 雷达能够扫描到两边墙时定位效果比较好，反之需要自己重新优化算法。
 * 苏凯
 * 2025-2-8
 */
using namespace yazi::json;
using namespace std;
using namespace Eigen;

// #include <Eigen/Eigen> 数据平滑
typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::Triplet<double> T;


// 定义颜色的转义序列 std::cerr
const std::string RED = "\033[31m";
const std::string GREEN = "\033[32m";
const std::string YELLOW = "\033[33m";
const std::string BLUE = "\033[34m";
const std::string MAGENTA = "\033[35m";
const std::string CYAN = "\033[36m";
const std::string RESET = "\033[0m";
std::mutex callback_mutex;
tf::TransformListener* tf_listener_new_;
tf::TransformBroadcaster *tf_broadcaster_new_;
std::chrono::time_point<std::chrono::high_resolution_clock> startTime_;
std::chrono::time_point<std::chrono::high_resolution_clock> trilateration_mapTbase_link_pose_startTime_;
double targetTrilaterationTimeOut_=10.0;//10s

// 11658，8531 控制 processQueue tf 发布
bool isRunProcessQueueFig_ =true;

#define ADD_HANDLER(handlers, handler) \
    handlers[#handler] = handler;
// service server 使用函数指针类型别名定义带有参数的函数指针类型别名，using：别名
using HandlerFunction = void (*)(const fanguangbantrilateration::trilateration_srvs::Request&, fanguangbantrilateration::trilateration_srvs::Response&);
//request_handlers
std::map<std::string, HandlerFunction>  request_handlers_;
//用于填充 request_handlers 映射表
std::map<std::string, HandlerFunction> createRequestHandlers();
//查找请求类型对应的处理函数,默认使用函数名即为 request_handlers_ 中的 key
void processRequest(const std::string& request_type, const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& res);
//接收用户指令的服务,"/trilateration_service"
bool trilaterationServiceCallback(fanguangbantrilateration::trilateration_srvs::Request &req,fanguangbantrilateration::trilateration_srvs::Response &res);

//toptic
std::string scan_toptic_name_="/scan";
std::string get_points_toptic_name_="/clicked_point";//取地图反光板点位
std::string initialpose_toptic_name_="/initialpose";//重定位
std::string show_marker_toptic_name_="/trilateration_marker";
std::string show_all_marker_toptic_name_="/trilateration_all_marker";
std::string show_all_current_marker_toptic_name_="/trilateration_all_current_marker";
std::string show_circle_points_toptic_name_="/trilateration_circle_points";
std::string service_name_="/trilateration_service"; //service 服务
std::string reflectormarker_marker_toptic_name_="/reflectormarker_marker";
std::string reflectormarker_path_marker_toptic_name_="/reflectormarker_path_marker";//匹配完成的连接线
//评估性能，发布性能评估数据
std::string time_log_topic_name_="/trilateration_time_log";
std::string getResultMapvalue_result_toptic_name_="/match_costmap_resultMapvalue";
std::string getCalculateScanMapRatio_result_toptic_name_="/match_costmap_calculateScanMapRatio";




int time_log_fig_=-1;// -1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 1：打开第一个，2：打开第二个 3：.....
int test_num_=-1;//参数服务测试使用，没有实际意思。通过这个参数可以动态的调试代码，把加入到代码调试位置
//参数服务
fanguangbantrilateration::TrilaterationParamConfig last_config_;
fanguangbantrilateration::TrilaterationParamConfig default_config_;
fanguangbantrilateration::TrilaterationParamConfig param_config_;
bool setup_=false;
std::mutex m_node_mutex_;

//发表匹配上的 marker 的唯一id；
int marker_id_=0;
// 雷达临时显示，发表marker 的唯一id；
int show_marker_id_=0;
//发表部署好的反光柱的唯一marker id；建图部署完成的反光柱
int show_all_marker_id_=0;
double  fangaungbanHeight_=0.11;//反光板厚度
double  fangaungbanWidth_=0.10;//反光板宽度
double  fangaungbanLength_=0.50;//0.001反光板高度设为一个很小的值，表示一个平面

//反光柱半径 90mm 阀值1500 LE50621
double  fangaungzhuRadius_ = 0.045;//反光柱半径

double  fangaungzhuHeight_ = 0.09;//反光柱厚度
double  fangaungzhuWidth_ = 0.09;//反光柱宽度
double  fangaungzhuLength_ = 0.50;//反光柱高度 0.001 设为一个很小的值，表示一个平面

//如果平均曲率超过阈值，认为是反光柱
double curvature_threshold_ = 0.01;
//在阀值距离范围内，判断 curvature_threshold_ 是反光柱还是反光板
double curvature_range_min_threshold_=1.5;//距离小于阀值有效
double curvature_range_max_threshold_=2.6;//距离大于阀值有效

//匹配上的反光板颜色
double  a_=1.0,r_=1.0,g_=1.0,b_=1.0;//3034 有颜色demo

// 全局互斥锁，用于保护文件操作
//std::mutex createReflectorPanelPointMutex;
int idCounter_ = 1; //生成反光板id用的
double matchok_timeout_ = 5.0;  // 反光板匹配上了,超时阈值，就剔除。单位为秒
bool isSendTf_=true;//是否发布定位的tf坐标
bool isFirtFanguangbanShowAllMaker_=true;//显示所有的的反光板点位,这个只会显示一次
bool is_trilateration_=true;//是否使用反光板定位，true时，使用反光板定位，false时，不使用反光板定位，订阅的scan中会return
int scan_size_ = 0.0;//获取雷达参数，只自动获取一次。
int intensities_size_ =  0.0;//获取雷达参数，只自动获取一次。
bool getScanParam_=true;//获取雷达参数，只自动获取一次。
//是否发布过滤后的只有强度数据的点云，雷达扫描到的反光柱轮廓
bool isSend_publish_circle_pointCloud_=true;

double edgesthreshold_ = 0.1;//反光板，柱 的特征匹配的边长特征的阀值
double anglesthreshold_ = 0.157;//反光板，柱 的特征匹配的两边夹角的阀值
//true：当有匹配上反光板后，优先使用匹配好的反光板数据。false：不使用匹配好的反光板数据，一直只使用反光板特征匹配算法
bool isReadMatchokReflectorPanel_=true;
//是否加入对比两线段的夹角作为匹配条件，true：加入，false：不加入
bool isStartAreAnglesEqualNew_=true;//true 加入夹角条件
bool  isShowMatchokReflectorFanGuanBanPoint_=true;//是否显示匹配完成的反光板点位
bool  isShowFanGuanBanPoint_=true; //是否显示反光板点位，一般部署会用到，核对反光板点位是否准确，不准确就动态调参数
bool  isShowFanGuanBanmarkerPath_=true;//是否显示反光板与小车之间的匹配连线

//判断是否是圆
double isCircular_threshold_ = 0.01;// 设置一个阈值，根据方差来判断是否近似为圆
//判断是否是直线
double isLine_threshold_ = 0.01;// 使用了较小的斜率阈值，使得只要拟合的直线接近水平，就认为这组点云近似于一条直线。
//点的阀值，点云聚类，分割出各个反光柱。至少连续三个点强度超过   强度阀值   即默认为反光柱,单独点滤除
int  points_size_threshold_=2;//至少连续三个点强度超过   强度阀值   即默认为反光柱,单独点滤除
//【过滤掉反光柱两俩之间距离太近（在 filter_tow_ont_distance_threshold_ 阀值内就剔除）的点（有可能是数据波动产生的）】
double filter_tow_ont_distance_threshold_ = 0.3;
int  findPolygonIndices_threshold_ = 2;//5725行 indices > 2至少要找到3个点，2个点的话才一条直线匹配错误高广度优先特征匹配
//todo  ------------------匹配--------------------------

bool isFindReflectorPanelPoints_vector_first_=true;//isFindReflectorPanelPoints_vector_=true 与 isFindRlectorPanelPointAllUnordered_maps_=true 同为true时有用；
//当通过key在map中查不到数据时是否遍历列表集合来查询数据,配合 reflectorPanelPoints_vector_size_ 使用，代表需要遍历前面多少数据，reflectorPanelPoints_vector_集合中的数据是安预测可能性排序的，可能性越大排序越靠前
bool isFindReflectorPanelPoints_vector_=true;
//是匹配时否搜索 reflectorPanelPointAllUnordered_maps中 的所有数据
bool isFindRlectorPanelPointAllUnordered_maps_=true;

int reflectorPanelPoints_vector_size_=8;// reflectorPanelPoints_vector 前 1/8 的数据
// Indicates whether the system is configured to use a third-party localization algorithm.
bool isUsingThirdPartyLocalization_;//是否配合第三方定位算法一起使用，比如 amcl等。true：表示一起使用
bool ispublishInitialPosefig_ = false;//是否操作的重定位


//记录是否已经通过3边定位成功了一次，第一次定位成功就设置true
bool  firstTrilateration_with_yawFig_=false;
// 定义一个方差阀值,判断当前通过3边定位算法出来的结果与里程计的方差是否小于阀值
double variance_yaw_threshold_ = 0.006;//角度方差
double variance_pose_threshold_ = 0.06;//位置方差


double variance_adjustment_yaw_threshold_ = 0.018;//满足角度调整阀值，可以设置 variance_yaw_threshold_ 的倍数作为参数
double variance_adjustment_pose_threshold_ = 0.3;//满足位置调整阀值，可以设置 variance_pose_threshold_ 的倍数作为参数

int isNotMin_varianceNum_target_=3;//连续出现3次定位失败就重新置为 firstTrilateration_with_yawFig_ = fasle。
int isNotMin_varianceNum_sourse_=0;//连续出现10次定位失败就重新置为 firstTrilateration_with_yawFig_ = fasle。

//判断是否有数据过来，超过时间没有数据认为，订阅不成功
std::chrono::time_point<std::chrono::high_resolution_clock> resultGetResultMapvalueCallback_startTime_;
//判断是否有数据过来，超过时间没有数据认为，订阅不成功
std::chrono::time_point<std::chrono::high_resolution_clock> resultGetCalculateScanMapRatioCallback_startTime_;

bool isGetResultMapvalueCallbackfig_=false;//判断 costmap 是否有数据过来
bool isGetCalculateScanMapRatioCallbackfig_=false;//判断 costmap 是否有数据过来
double matchcostmapwithscanTimeOut_=3.0;//超时说明没有订阅到coatmap的数据
int   free_cell_ = 0;// 0代表空闲
int   occupied_cell_ = 100;//100代表占用
int   unknown_cell_ = -1;// -1代表未知

//todo 2. 雷达匹与周边特征的配情况。 注意： 非长走廊环境需要自己改一下代码，不要使用这个节来取置信度数据。
double matchingcostma_radius_ = 0.1;//搜索半径
double matchingcostma_threshold_ = 0.8;//比例阀值，超过这个比例认为雷达匹配的挺好

int result_Mapvalue_=-3;//代价值
int matching_points_ = 0;//匹配上的雷达点数量
int scan_points_map_size_=0;//所有雷达点数量
int total_points_size_=0;//雷达点数据量的有效数据量
bool is_ratio_ =false;//是否满足条件

bool is_use_scan_points_map_size_ =true;//true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。false 可以尝试在宽阔且雷达打不到墙的环境中使用，需要满足一部分雷达点能打到墙壁。
bool is_start_place_rotation_=true;//是否启动判断是否是原地旋转功能，启动后 原地旋转 不会更新定位数据
int rotate_in_place_variance_multiplier_ = 4;//is_start_place_rotation=true 后会加载 rotate_in_place_variance_multiplier 在原地旋转时放大定位精度，从而减少定位的抖动带来的移动机器人反复调整位置.

//  从 /home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/src/test/test52.cpp中 是否发布数据
bool isPublish_matchcostmapwithscan_mssage_=false;//是否发布数据，true：会订阅到 "/match_costmap_resultMapvalue" ，"/match_costmap_calculateScanMapRatio" 这些数据
std::string match_costmap_with_scan_service_name_="/match_costmap_with_scan_service";
// 创建一个服务客户端，指定服务名称和服务消息类型
ros::ServiceClient match_costmap_with_scan_service_client_ ;
//-----------------------------------------------------------------------------------------------------------------------------------------------

std::string fileOutPut_="/home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/output";
//储存所有反光板点位到txt
std::string savePointFilePath_ = "/home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/output/allpoint.txt";
//储存计算好的数据到txt，2个点计算一条直线
std::string calculateFilePath_ = "/home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/output/calculate.txt";
//储存雷达强度数据
std::string scanIntensitiesFilePath_ = "/home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/output/scanIntensities.txt";

std::string mapTbaselinkPose_path_="/home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/output/mapTbaselinkPose.txt";

// 全局互斥锁，用于保护文件操作
std::mutex fileMutex;
std::mutex mapMutex;
std::mutex filePointMutex;
std::mutex mapPointMutex;



int end_scan_index_=9999999;//雷达扫描结束点索引

double scan_intensitie_=1500.0;//雷达强度值
double intensitie_threshole_=100;//雷达强度阀值
double range_threshole_=0.1;//雷达扫描距离阀值

int time_milliseconds_=100;//匹配上的点的更新速度 ms,点为marker的发布频率
double scan_distances_threshole_=10.0;//超过这个值为无效数据，雷达有效范围，也在预测范围内，单位m

//todo 定义哈希函数，map中key的允许误差，key是两个double浮点数 1516
double epsilon_ = 0.11;//todo  重要数据 误差精度影响map的key搜索，误差不能大于两个反光板直接的距离

std::vector<int> scan_intensitie_key_;//int:雷达距离 ，索引没用
std::map<int,double> scan_intensitie_map_;//int:雷达距离 value:雷达强度，   储存距离对应雷达强度数据

ros::Publisher  pub_marker_,pub_circle_pointCloud_,time_log_pub_,initial_pose_pub_;
ros::Publisher  pub_all_marker_,pub_all_current_marker_;
std::string map_frame_id_="map";//地图坐标系
std::string base_link_frame_id_="base_link";//小车坐标系
std::string odom_frame_id_="odom";//odom坐标系
std::string laser_frame_id_="laser";// base_scan laser  雷达坐标系
std::string imu_frame_id_="imu_link";//imu坐标系
//===================================================================================================
tf::Transform tf_last_odom_to_map_transform_;//储存最后一次tf定位数据
//todo 存储上一侦计算得到的定位数据3边定位最小二乘
tf::Transform up_mapTbase_link_trilateration_;
double mapTbase_link_pos_threshold_ = 0.1; //3边定位最小二乘 计算得到的 mapTbase_link  位置阈值
double mapTbase_link_rot_threshold_ = 0.15; //3边定位最小二乘 计算得到的  角度阈值，例如10度

//三边定位算出的移动机器人在map下的坐标 （核实准确性使用）
std::string mapTcurrentCalculate_sanbian_BaseLinkPose_frame_id_="mapTcurrentCalculate_BaseLinkPose_sanbian";
//tf位姿推算出的移动机器人在map下的坐标（核实准确性使用）
std::string mapTcurrentCalculate_tf_BaseLinkPose_frame_id_="mapTcurrentCalculate_tf_BaseLinkPose_tf";

ros::Publisher  pub_point1_,pub_point2_,pub_point3_,pub_point4_,pub_point5_;
// 全局容器管理线程
std::vector<std::shared_ptr<std::thread>> threadPool_;
//控制线程
bool iswileTure_=true;

//计算特征时的距离阀值，计算距离阀值，两点过远就不需要计算了
double calculateEdge_threshold_=5.0;

bool iswirtortSetPeriodically_=false;//当前true时表示有函数在操作 reflectorPanelPoints_vector_
//加入已经匹配上的反光板,禁止查询
bool iswirtMatchokReflectoUnordered_map_=false;

tf::StampedTransform odom_to_map_transform_;//mapTodom
tf::StampedTransform base_link_to_odom_transform_;//odomTbase_link
tf::StampedTransform base_link_to_map_transform_;//mapTbase_link
tf::StampedTransform laser_to_base_link_transform_;//baselinkTlaser
//todo laserTodom 获取 odom 到 laser 的变换  laserTodom
tf::StampedTransform now_odomTlaser_transform_;//laserTodom

// 初始化上一帧的位姿变换
tf::StampedTransform previous_base_link_to_odom_transform_ ;
bool isfirstPrevious_base_link_to_odom_transform_=true;

// 初始化上一帧的位姿变换
tf::StampedTransform previous_base_link_to_map_transform_ ;
bool isfirstPrevious_base_link_to_map_transform_=true;




// todo mapTbase_link 经典三边定位最小二乘法 计算得到的 mapTbase_link
tf::Transform mapTbase_link_trilateration_new_;
tf::Transform mapTbase_link_old_trilateration_;

//中位数滤波器 窗口大小
int window_size_ =5;//中位数滤波器
//低通滤波器 alpha 值为 0.1
double alpha_ =0.1;//1.0

bool isTrilateration_with_yawFig_=true; //最小二乘法3边定位，程序启动的第一次用

//判断是否是原地旋转 CheckInPlaceRotation 原地转到的 位移在0.0001-0.0003之间，角度大于0.01  , 行走时的位移在0.01-0.02之间，角度大于等于0.0001
double angle_threshold_ = 0.001; // 角度阈值（弧度）
double distance_threshold_ = 0.005 ;  // 位移阈值（米）
//超过这个精度就使用反光柱定位数据进行定位
//这里判断计算出来的定位与上一次的定位误差值，小于 某个容忍阀值 就不更新定位，减少定位波动（波动会影响机器人运动的卡顿情况）。需要大于一定阀值时才会使用新的定位数据。
double  variance_min_yaw_lerance_=0.03,  variance_min_pose_lerance_=0.03;

//如果score大于1.2且小于等于3.0，调用CheckResultValid函数来验证匹配结果是否有效。
double  score_min_threshold_=1.2,  score_max_threshold_=3.0;// ComputeCurrentPose 这里输入为匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿

//=======================================

// 定义一个函数用于转换
tf::Transform poseToTransform(const geometry_msgs::Pose& pose) {
    // 创建 tf::Transform 实例
    tf::Transform transform;

    // 设置位置
    transform.setOrigin(tf::Vector3(pose.position.x, pose.position.y, pose.position.z));

    // 设置旋转
    tf::Quaternion q(
            pose.orientation.x,
            pose.orientation.y,
            pose.orientation.z,
            pose.orientation.w
    );
    transform.setRotation(q);

    return transform;
}
// 定义一个函数用于转换
geometry_msgs::Pose transformToPose(const tf::Transform& transform) {
    // 创建 geometry_msgs::Pose 实例
    geometry_msgs::Pose pose;

    // 设置位置
    pose.position.x = transform.getOrigin().x();
    pose.position.y = transform.getOrigin().y();
    pose.position.z = transform.getOrigin().z();

    // 设置旋转
    pose.orientation.x = transform.getRotation().getX();
    pose.orientation.y = transform.getRotation().getY();
    pose.orientation.z = transform.getRotation().getZ();
    pose.orientation.w = transform.getRotation().getW();

    return pose;
}


//===================================

// 残差计算类
struct TrilaterationResidual {
    TrilaterationResidual(double observed_x, double observed_y, double measured_distance)
            : observed_x_(observed_x), observed_y_(observed_y), d3_(measured_distance) {}

    template <typename T>
    bool operator()(const T* const pose, T* residual) const {
        // 计算基于优化后的x, y的预测距离
        T predicted_distance = ceres::sqrt(ceres::pow(observed_x_ - pose[0], T(2)) +
                                           ceres::pow(observed_y_ - pose[1], T(2)));
        // 残差是预测距离与测量距离之间的差异
        // residual[0] = predicted_distance - T(d3_);
        //这里可以改进
        T angle_diff = ceres::atan2(observed_y_ - pose[1], observed_x_ - pose[0]) - pose[2];

        // 引入角度误差的加权
        T weight = T(1.0) + T(0.1) * ceres::abs(angle_diff);
        residual[0] = weight * (predicted_distance - T(d3_));

        return true;
    }

private:
    const double observed_x_;
    const double observed_y_;
    const double d3_;
};
typedef struct FeaturePairCost {
    FeaturePairCost(pair<double, double> pt1, pair<double, double> pt2) : _pt1(pt1), _pt2(pt2) {}

    template <typename T>
    bool operator()(const T* const pose, T* residual) const {

        //   _pt1.first 对应  local_coords_fpt_pair_second
        //  _pt1.second 对应  global_coords_fpt_pair_first
        T x_tr = T(_pt1.first) * cos(pose[2]) - T(_pt1.second) * sin(pose[2]) + pose[0];
        T y_tr = T(_pt1.first) * sin(pose[2]) + T(_pt1.second) * cos(pose[2]) + pose[1];

//        residual[0] = x_tr - T(_pt2.first);
//        residual[1] = y_tr - T(_pt2.second);


        // 结合旋转的误差平滑
        T rotation_effect = T(1.0) / (T(1.0) + T(0.5) * ceres::abs(pose[2]));
        residual[0] = rotation_effect * (x_tr - T(_pt2.first));
        residual[1] = rotation_effect * (y_tr - T(_pt2.second));


        return true;
    }

    const pair<double, double> _pt1, _pt2;
} FeaturePairCost;


//========================================
typedef struct ComplexResidual {
    ComplexResidual(
            pair<double, double> observed_point_first,
            pair<double, double> observed_point_first2,
            pair<double, double> observed_point_second,
            pair<double, double> observed_point_second2,
            double measured_distance,
            const tf::StampedTransform& previous_transform,
            bool is_in_place_rotation)
            : local_coords_fpt_pair_second_(observed_point_first),//local_coords_fpt_pair_second
              local_coords_fpt_pair_second2_(observed_point_first2),//local_coords_fpt_pair_second2
              global_coords_fpt_pair_first_(observed_point_second),//global_coords_fpt_pair_first
              global_coords_fpt_pair_first2_(observed_point_second2),//global_coords_fpt_pair_first2
              d3_(measured_distance),//d3
              previous_base_link_to_odom_transform_(previous_transform),//previous_transform
              is_in_place_rotation_(is_in_place_rotation) {}

public:
    template <typename T>
    T NormalizeAngle(const T& angle) const {
        T two_pi = T(2.0 * M_PI);
        T normalized_angle = angle - two_pi * ceres::floor((angle + T(M_PI)) / two_pi);
        return normalized_angle;
    }


    template <typename T>
    bool operator()(const T* const pose, T* residual) const {
        // 当前帧的预测位置，当前的 base_link_to_map_transform_ 位置数据
        T predicted_x = pose[0];
        T predicted_y = pose[1];
        T predicted_yaw =pose[2];
        // 使用自定义函数将 predicted_yaw 归一化到 [-M_PI, M_PI]
        predicted_yaw = NormalizeAngle(predicted_yaw);//// predicted_yaw = ceres::fmod(predicted_yaw + M_PI, 2 * M_PI) - M_PI;


        // 第一组观测点的旋转后的坐标
        //local_coords_fpt_pair_second_ ： 雷达探测到的反光柱在baselink下的坐标，local_coords_fpt_pair_second_.first：x，local_coords_fpt_pair_second_.second：y
        T rotated_x1 = T(local_coords_fpt_pair_second_.first) * cos(predicted_yaw) - T(local_coords_fpt_pair_second_.second) * sin(predicted_yaw) + predicted_x;
        T rotated_y1 = T(local_coords_fpt_pair_second_.first) * sin(predicted_yaw) + T(local_coords_fpt_pair_second_.second) * cos(predicted_yaw) + predicted_y;

        // 第二组观测点的旋转后的坐标
//        T rotated_x2 = T(local_coords_fpt_pair_second2_.first) * cos(predicted_yaw) - T(local_coords_fpt_pair_second2_.second) * sin(predicted_yaw) + predicted_x;
//        T rotated_y2 = T(local_coords_fpt_pair_second2_.first) * sin(predicted_yaw) + T(local_coords_fpt_pair_second2_.second) * cos(predicted_yaw) + predicted_y;

//        // su 计算移动机器人姿态
//        // 反光柱在地图中的坐标
//        T refX = T(local_coords_fpt_pair_second_.first) - T(local_coords_fpt_pair_second2_.first);//yaw2 -map
//        T refY = T(local_coords_fpt_pair_second_.second) - T(local_coords_fpt_pair_second2_.second);//yaw2 -map
//        // su 反光柱的在 baselink下的坐标(current_baselinkTscan_x, current_baselinkTscan_y)
//        T scan_x1 = T(global_coords_fpt_pair_first_.first) - T(global_coords_fpt_pair_first2_.first);//yaw1 -baselink
//        T scan_y1 = T(global_coords_fpt_pair_first_.second) - T(global_coords_fpt_pair_first2_.second);//yaw1 -baselink

        // su 计算移动机器人姿态
        // 反光柱在地图中的坐标 ， local_coords_fpt_pair_second_ ： 雷达探测到的反光柱在baselink下的坐标；global_coords_fpt_pair_first2_：部署在地图中的反光柱全局坐标
        T refX = T(global_coords_fpt_pair_first_.first) - T(global_coords_fpt_pair_first2_.first);//yaw2 -map
        T refY = T(global_coords_fpt_pair_first_.second) - T(global_coords_fpt_pair_first2_.second);//yaw2 -map
        // su 反光柱的在 baselink下的坐标(current_baselinkTscan_x, current_baselinkTscan_y)
        T scan_x1 = T(local_coords_fpt_pair_second_.first) - T(local_coords_fpt_pair_second2_.first);//yaw1 -baselink
        T scan_y1 = T(local_coords_fpt_pair_second_.second) - T(local_coords_fpt_pair_second2_.second);//yaw1 -baselink


        T yaw2  = atan2(refY, refX); // 向量在 map 坐标系中的姿态
        T yaw1  = atan2(scan_y1, scan_x1);// 向量在 baselink 坐标系中的姿态
        T yaw =  yaw2-yaw1;//小车baselink的姿态=向量在 map 坐标系中的姿态-向量在 baselink 坐标系中的姿态 【 yaw = yaw2 - yaw1 】
        //这段代码确保yaw的值在[-π, π]范围内。fmod函数用于取模运算，这里的目的是将偏航角差值映射回标准的2π周期内。首先加上π，然后对2π取模，最后减去π，确保结果在[-π, π]之间。6.28
        // yaw = fmod(yaw + M_PI, 2 * M_PI) - M_PI;//mapTbaselink_yaw
        yaw = NormalizeAngle(yaw);


        // 上一帧的位姿变化  previous_base_link_to_odom_transform_
        T previous_x = T(previous_base_link_to_odom_transform_.getOrigin().x());
        T previous_y = T(previous_base_link_to_odom_transform_.getOrigin().y());
        T previous_yaw = T(tf::getYaw(previous_base_link_to_odom_transform_.getRotation()));
        previous_yaw = NormalizeAngle(previous_yaw);//// predicted_yaw = ceres::fmod(predicted_yaw + M_PI, 2 * M_PI) - M_PI;

        // 计算预测的距离
        //T predicted_distance = ceres::sqrt(ceres::pow(rotated_x1 - previous_x, T(2)) + ceres::pow(rotated_y1 - previous_y, T(2)));
        T predicted_distance = ceres::sqrt(ceres::pow(global_coords_fpt_pair_first_.first - predicted_x, T(2)) + ceres::pow(global_coords_fpt_pair_first_.second - predicted_y, T(2)));

        // 计算旋转惩罚
        //  T rotation_penalty = T(0);
        // if (is_in_place_rotation_) {
        //  rotation_penalty = ceres::abs(predicted_yaw - previous_yaw);
        //}
        // rotation_penalty = ceres::abs(yaw - predicted_yaw);

        // 计算 yaw 角度差
//        T yaw_diff_current = NormalizeAngle(predicted_yaw - yaw);
//        T yaw_diff_previous = NormalizeAngle(predicted_yaw - previous_yaw);

        T yaw_diff_current = NormalizeAngle ( yaw - predicted_yaw);
        // T yaw_diff_previous = NormalizeAngle( predicted_yaw - previous_yaw);

        // 应用旋转的误差平滑因子
        // T rotation_effect = T(1.0) / (T(1.0) + T(0.5) * (ceres::abs(yaw_diff_current) + ceres::abs(yaw_diff_previous)));
        T rotation_effect = T(1.0) / (T(1.0) + T(0.5) * ceres::abs(yaw_diff_current) );
        // 残差计算
        residual[0] = rotation_effect * (predicted_distance - T(d3_));
        residual[1] = rotation_effect * (rotated_x1 - T(global_coords_fpt_pair_first_.first));
        residual[2] = rotation_effect * (rotated_y1 - T(global_coords_fpt_pair_first_.second));
        residual[3] = rotation_effect * yaw_diff_current;




        // 更新残差计算，平滑旋转误差
//        residual[0] = rotation_effect *(predicted_distance - T(d3_));
//        residual[1] = rotation_effect * (rotated_x1 - T(global_coords_fpt_pair_first_.first));
//        residual[2] = rotation_effect * (rotated_y1 - T(global_coords_fpt_pair_first_.second));
//        residual[3] = rotation_effect * (  yaw - predicted_yaw); // 这里 residual[2] 表示旋转的残差


        //residual[2] = rotation_effect * (predicted_yaw - previous_yaw); // 这里 residual[2] 表示旋转的残差


//        residual[0] = rotation_effect * (rotated_x1 - previous_x);
//        residual[1] = rotation_effect * (rotated_y1 - previous_y);
        return true;
    }

private:
    const pair<double, double> local_coords_fpt_pair_second_;
    const pair<double, double> local_coords_fpt_pair_second2_;
    const pair<double, double> global_coords_fpt_pair_first_;
    const pair<double, double> global_coords_fpt_pair_first2_;
    const double d3_;
    const tf::StampedTransform& previous_base_link_to_odom_transform_;
    const bool is_in_place_rotation_;
} ComplexResidual;


//======================================

// 中位数滤波器
class MedianFilter {
public:
    MedianFilter(size_t window_size) : window_size_(window_size) {}

    double filter(double new_value) {
        if (values_.size() >= window_size_) {
            values_.pop_back();
        }
        values_.insert(values_.begin(), new_value);

        std::vector<double> sorted_values = values_;
        std::sort(sorted_values.begin(), sorted_values.end());

        size_t median_index = sorted_values.size() / 2;
        return sorted_values[median_index];
    }

private:
    size_t window_size_;
    std::vector<double> values_;
};

// 低通滤波器
class LowPassFilter {
public:
    LowPassFilter(double alpha) : alpha_(alpha), prev_value_(0.0) {}

    double filter(double new_value) {
        double filtered_value = alpha_ * new_value + (1 - alpha_) * prev_value_;
        prev_value_ = filtered_value;
        return filtered_value;
    }

private:
    double alpha_;
    double prev_value_;
};

















//====================================
//多边形质心point
struct CentroidPoint {
    double x=0.0, y=0.0,z=0.0;
};

struct Point {
    tf::StampedTransform odomTlaser_transform_1;
    tf::StampedTransform transform_map_laser_1;
    tf::StampedTransform odomTbase_link_transform_1;
    // 通过雷达探测到的实时 map 坐标系下反光柱坐标
    double x=0.0, y=0.0,z=0.0,yaw=0.0;
    double rx=0.0, ry=0.0,rz=0.0,rw=1.0;
    // 通过雷达探测到的实时 odom坐标系下的反光柱坐标
    double odomTscan_x=0.0, odomTscan_y=0.0,odomTscan_z=0.0,odomTscan_yaw=0.0;
    double odomTscan_rx=0.0, odomTscan_ry=0.0,odomTscan_rz=0.0,odomTscan_rw=1.0;

    //[时时扫描到的数据] 反光柱在激光雷达坐标系下的坐标 laserTscanPoint
    double laserTscan_x=0.0, laserTscan_y=0.0,laserTscan_z=0.0,laserTscan_yaw=0.0;
    double laserTscan_rx=0.0, laserTscan_ry=0.0,laserTscan_rz=0.0,laserTscan_rw=1.0;

    int scan_index=0;//雷达索引
    double range=0.0;//反光板到 雷达 的距离

    double intensity=0.0;
    //int scan_angle=0;//雷达扫描角度
    std::string shape="CYLINDER";//CUBE：立方体（平面）,CYLINDER: 圆柱体
    std::string id="EMPTY";// 简化的10位字符串生成器




};

struct Pose {
    std::string id="EMPTY";
    Point position;
    // double yaw=0.0;  // 偏航角，以弧度表示
};
//------------------------------------------


//------------------------------------------
//临时储存匹配数据
struct ReflectorPanelPointStruct {


    std::string id="EMPTY";//唯一标示
    std::string shape="CYLINDER";//CUBE：立方体（平面）,CYLINDER: 圆柱体
    Point circle;//雷达探测到的反光柱数据
    //显示用的
    int reflectorPanelPoint_marker_id_=0;
    int reflectorPanelPoint_text_id_=0;
    int scan_index=0;//雷达索引
    double range=0.0;//反光板到 雷达 的距离， 定位丢失时这个数据是可靠的
    //---------------------------------------------------------------------------------------
    //反光板到 base_link 的距离 [通过坐标变换计算的到的距离，涉及到了map，所以这个距离是依赖先有准确定位才行]
    double distances=0.0;//计算base_link坐标系下 rx=0.0, ry=0.0, rz=0.0, rw=1.0 欧几里德距离
    //未基于map坐标系计算的【定位丢失时这个数据是可靠的（未涉及到了map坐标系， 是基于 lase - > baselink 计算的）】
    double base_linkTfanguangban_distances=0.0;//反光板到 base_link 的距离
    //反光板到 mind 的距离， mind是 base_link 与 laser两点连线的点。【定位丢失时这个数据是可靠的（未涉及到了map坐标系， 是基于 lase - > baselink 计算的）】
    double mindTfanguangban_distances = 0.0;//【计算yaw 姿态用，两点构成一条向量，向量是有方向的】
    //------------------------------------------------------------------------------


    double intensity=0.0;
    int scan_angle=0;//雷达扫描角度

    int isMatchok_=-1;//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    int currentIsMatchok_=-1;//-1：未开始匹配  0： laserTscan_x 已经被使用了，laserTscan_x这数据需只能通过tf或odom的变化量来预测更新成为最新的数据  1:匹配上并给雷达 laserTscan_x 数据赋值了
    int isAssignment_=-1;//1:不可以赋值 ，-1：可以赋值
    ros::Time last_seen_;//记录匹配上的时间
    tf::StampedTransform last_laser_to_odom_transform_;//  odomTlaser 记录匹配上时的历程数据
    tf::StampedTransform  last_mapTlaser_transform_;//记录匹配上时的历程数据 transform_map_laser_1 last_mapTbase_link_transform_
    tf::StampedTransform  last_odomTbase_link_transform_;
    tf::StampedTransform  last_mapTbase_link_transform_;
    std::string source_frame_id=base_link_frame_id_; //base_link_frame_id_  base_link_frame_id_
    std::string target_frame_id=map_frame_id_;//laser laser_frame_id_ base_link_frame_id_
    //  //部署的反光柱数据 在map坐标系下的反光柱数据  mapTscanPoin mapTfanguangban
    double x=0.0, y=0.0, z=0.0,yaw=0.0;// yaw = mapTodom_yaw+ odomTbaselink
    double rx=0.0, ry=0.0, rz=0.0, rw=1.0;
    double mapTodom_yaw = 0.0;//odom在map下的姿态。 mapTodom_yaw=yaw * odomTbaselink的逆

    // 部署在环境中的反光板在 base_link 下的坐标
    // base_linkTscanPoin  base_linkTfanguangban t  把’在map坐标系下的雷达数‘ x=0.0, y=0.0, z=0.0。 换算到laser坐标系下 scan_x=0.0, scan_y=0.0, scan_z=0.0。
    // 这样就可以跟踪到 scan_x 离雷达最近，且scan_x>0的数据=》永远在雷达的正方向，安排序规则来排序 scan_x>=0且值最小。
    double scan_x=0.0, scan_y=0.0, scan_z=0.0,scan_yaw=0.0;
    double scan_rx=0.0, scan_ry=0.0, scan_rz=0.0, scan_rw=1.0;


    //反光柱在激光雷达坐标系下的坐标 laserTscanPoint 【这里的数据是需要特征匹配上之后才会被赋值，即 isMatchok_ = 1 】
    double laserTscan_x=0.0, laserTscan_y=0.0,laserTscan_z=0.0,laserTscan_yaw=0.0;
    double laserTscan_rx=0.0, laserTscan_ry=0.0,laserTscan_rz=0.0,laserTscan_rw=1.0;

    //通过tf预测 当前反光柱在激光雷达坐标系下的坐标 laserTscanPoint 【这里的数据是需要特征匹配上之后才会被赋值，即 isMatchok_ = 1 】
    double forecast_laserTscan_x=0.0, forecast_laserTscan_y=0.0,forecast_laserTscan_z=0.0,forecast_laserTscan_yaw=0.0;
    double forecast_laserTscan_rx=0.0, forecast_laserTscan_ry=0.0,forecast_laserTscan_rz=0.0,forecast_laserTscan_rw=1.0;


    //当前时时扫描到的反光柱在baselink下的坐标
    double  current_baselinkTscan_x=0.0, current_baselinkTscan_y=0.0, current_baselinkTscan_z=0.0,current_baselinkTscan_yaw=0.0;
    double  current_baselinkTscan_rx=0.0, current_baselinkTscan_ry=0.0, current_baselinkTscan_rz=0.0, current_baselinkTscan_rw=1.0;


    double angle=0.0;//计算laser坐标系下的角度 scan_x=0.0, scan_y=0.0, scan_z



};


//----------------------------------------------

// 简化的10位字符串生成器
std::string generate_shortened_string() {
    // 获取当前时间（秒精度）
    auto now = std::chrono::system_clock::now();
    auto time_point_s = std::chrono::time_point_cast<std::chrono::seconds>(now);
    uint32_t timestamp = time_point_s.time_since_epoch().count();

    // 添加随机数
    std::random_device rd;
    std::mt19937 gen(rd());

    // 产生一个0-25之间的随机数，对应字母'A'到'Z'
    std::uniform_int_distribution<> letter_dis(0, 25);
    char random_letter = 'A' + letter_dis(gen);

    // 产生一个0-999999之间的随机数
    std::uniform_int_distribution<> dis(0, 999999);

    std::stringstream ss;//#include <sstream>
    // 首字符为随机大写字母，后面跟上原本的5-5格式数字
    ss << random_letter
       << std::uppercase << std::hex << std::setw(5) << std::setfill('0') << timestamp
       << std::hex << std::setw(5) << std::setfill('0') << dis(gen);
    return ss.str();
}

//===========================================================================
#ifndef REFLECTOR_PANEL_POINT_H
#define REFLECTOR_PANEL_POINT_H
class ReflectorPanelPoint {
public:
    Point circle;//雷达探测到的反光柱数据
    std::string id="EMPTY";//唯一标示
    std::string shape="CYLINDER";//CUBE：立方体（平面）,CYLINDER: 圆柱体
    //显示用的
    int reflectorPanelPoint_marker_id_=0;
    int reflectorPanelPoint_text_id_=0;
    int scan_index=0;//雷达索引
    double range=0.0;//反光板到 雷达 的距离， 定位丢失时这个数据是可靠的
    //---------------------------------------------------------------------------------------
    //反光板到 base_link 的距离 [通过坐标变换计算的到的距离，涉及到了map，所以这个距离是依赖先有准确定位才行]
    double distances=0.0;//计算base_link坐标系下 rx=0.0, ry=0.0, rz=0.0, rw=1.0 欧几里德距离
    //未基于map坐标系计算的【定位丢失时这个数据是可靠的（未涉及到了map坐标系， 是基于 lase - > baselink 计算的）】
    double base_linkTfanguangban_distances=0.0;//反光板到 base_link 的距离
    //反光板到 mind 的距离， mind是 base_link 与 laser两点连线的点。【定位丢失时这个数据是可靠的（未涉及到了map坐标系， 是基于 lase - > baselink 计算的）】
    double mindTfanguangban_distances = 0.0;//【计算yaw 姿态用，两点构成一条向量，向量是有方向的】
    //------------------------------------------------------------------------------


    double intensity=0.0;
    int scan_angle=0;//雷达扫描角度

    int isMatchok_=-1;//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    int currentIsMatchok_=-1;//-1：未开始匹配  0： laserTscan_x 已经被使用了，laserTscan_x这数据需只能通过tf或odom的变化量来预测更新成为最新的数据  1:匹配上并给雷达 laserTscan_x 数据赋值了
    int isAssignment_=-1;//1:不可以赋值 ，-1：可以赋值
    ros::Time last_seen_;//记录匹配上的时间
    tf::StampedTransform  last_laser_to_odom_transform_;//  odomTlaser 记录匹配上时的历程数据
    tf::StampedTransform  last_mapTlaser_transform_;//记录匹配上时的历程数据
    tf::StampedTransform  last_odomTbase_link_transform_;
    tf::StampedTransform  last_mapTbase_link_transform_;
    std::string source_frame_id=base_link_frame_id_; //base_link_frame_id_  base_link_frame_id_
    std::string target_frame_id=map_frame_id_;//laser laser_frame_id_ base_link_frame_id_
    // 在map坐标系下的雷达数据  mapTscanPoin mapTfanguangban
    double x=0.0, y=0.0, z=0.0,yaw=0.0;// yaw = mapTodom_yaw+ odomTbaselink
    double rx=0.0, ry=0.0, rz=0.0, rw=1.0;
    double mapTodom_yaw = 0.0;//odom在map下的姿态。 mapTodom_yaw=yaw * odomTbaselink的逆

    // 部署在环境中的反光板在 base_link 下的坐标
    // base_linkTscanPoin  base_linkTfanguangban t  把’在map坐标系下的雷达数‘ x=0.0, y=0.0, z=0.0。 换算到laser坐标系下 scan_x=0.0, scan_y=0.0, scan_z=0.0。
    // 这样就可以跟踪到 scan_x 离雷达最近，且scan_x>0的数据=》永远在雷达的正方向，安排序规则来排序 scan_x>=0且值最小。
    double scan_x=0.0, scan_y=0.0, scan_z=0.0,scan_yaw=0.0;
    double scan_rx=0.0, scan_ry=0.0, scan_rz=0.0, scan_rw=1.0;


    //反光柱在激光雷达坐标系下的坐标 laserTscanPoint 【这里的数据是需要特征匹配上之后才会被赋值，即 isMatchok_ = 1 】
    double laserTscan_x=0.0, laserTscan_y=0.0,laserTscan_z=0.0,laserTscan_yaw=0.0;
    double laserTscan_rx=0.0, laserTscan_ry=0.0,laserTscan_rz=0.0,laserTscan_rw=1.0;

    //通过tf预测 当前反光柱在激光雷达坐标系下的坐标 laserTscanPoint 【这里的数据是需要特征匹配上之后才会被赋值，即 isMatchok_ = 1 】
    double forecast_laserTscan_x=0.0, forecast_laserTscan_y=0.0,forecast_laserTscan_z=0.0,forecast_laserTscan_yaw=0.0;
    double forecast_laserTscan_rx=0.0, forecast_laserTscan_ry=0.0,forecast_laserTscan_rz=0.0,forecast_laserTscan_rw=1.0;


    //当前时时扫描到的反光柱在baselink下的坐标
    double  current_baselinkTscan_x=0.0, current_baselinkTscan_y=0.0, current_baselinkTscan_z=0.0,current_baselinkTscan_yaw=0.0;
    double  current_baselinkTscan_rx=0.0, current_baselinkTscan_ry=0.0, current_baselinkTscan_rz=0.0, current_baselinkTscan_rw=1.0;


    double angle=0.0;//计算laser坐标系下的角度 scan_x=0.0, scan_y=0.0, scan_z

    ros::NodeHandle nh_;
    //private:
    //tf::TransformListener tf_listener_;  //这里使用 tf 会把 cpu 计算量增加40%
    // 创建一个tf::TransformListener实例
    //  tf::TransformListener tf_listener_odom_;
    // 增加一个ros::Subscriber对象
    ros::Subscriber odom_sub_;
    ros::Publisher pub_reflectormarker,path_marker_pub;
    //多线程1 base_link 到扫描到的雷达的距离 distances 。
    std::atomic<bool> should_stop{false}; // 用于线程停止的信号
    std::thread transform_thread;
    std::mutex reflectorPanelPoint_transformAndCalculate_mutex_;
    //多线程2 时时更新雷达扫描到的反光板位置
    std::atomic<bool> should_stop2{false}; // 用于线程停止的信号
    std::thread transform_thread2;
    std::mutex reflectorPanelPoint_transformAndCalculate_mutex2_;

    explicit ReflectorPanelPoint(const std::string& id);
    explicit ReflectorPanelPoint(const std::string& id,int marker_id, double x, double y, double z, double rx, double ry, double rz, double rw);
    explicit ReflectorPanelPoint(const std::string& id,int marker_id, double x, double y, double z, double rx, double ry, double rz, double rw,string &shape,int &reflectorPanelPoint_text_id);

    ReflectorPanelPoint();

    ~ReflectorPanelPoint() {
        isMatchok_=-1;//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
        currentIsMatchok_=-1;//-1：未开始匹配  0： laserTscan_x 已经被使用了，laserTscan_x这数据需只能通过tf或odom的变化量来预测更新成为最新的数据  1:匹配上并给雷达 laserTscan_x 数据赋值了
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        stopTransformThread();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        stopTransformThread2();
    }

public:
    static std::unordered_map<std::string, std::shared_ptr<ReflectorPanelPoint>> instances;
    static std::mutex instance_mutex;
    static bool deleteInstance(const std::string& id);
    static bool deleteAllInstance();
    //初始化数据
    static void initInstance();
    static bool getInstance(const std::string& id,std::shared_ptr<ReflectorPanelPoint> & reflectorPanelPoint);
    static std::shared_ptr<ReflectorPanelPoint> createInstance1(const std::string& id);
    static std::shared_ptr<ReflectorPanelPoint> createInstance2(const std::string& id,int marker_id, double x, double y, double z, double rx, double ry, double rz, double rw);
    static std::shared_ptr<ReflectorPanelPoint> createInstance3(const std::string& id,int marker_id, double x, double y, double z, double rx, double ry, double rz, double rw,string &shape,int &reflectorPanelPoint_text_id);
    //多线程1 base_link 到扫描到的雷达的距离 distances 。
    void startTransformThread(); // Assuming this function is defined elsewhere,transformThreadFunc()->transformAndCalculate()
    void stopTransformThread();
    void transformAndCalculate();
    void transformThreadFunc();
    //多线程2 时时更新雷达扫描到的反光板位置
    void startTransformThread2(); // Assuming this function is defined elsewhere
    void stopTransformThread2();
    void transformAndCalculate2();
    void transformThreadFunc2();
    void initparam();
    // 在类创建rostoptic ros::NodeHandle& nh
    void careatrRosNode();
    //订阅雷达数
    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
    //三角函数拟定圆心，根据公式原理编写用三角函数法拟定圆心的函数。
    Point trigonometry(std::vector< Point >  &points );
    //查找高强度点组作为反光柱并确定圆心
    std::vector< Point > findCircle (const sensor_msgs::LaserScan &scan);
    // 计算点到圆心的距离的平方
    double distanceSquared(const Point& p1, const Point& p2);
    // 判断点云是否代表一个圆弧
    bool isCircular(const std::vector<Point>& points);

    //--------------------------判断是否是直线--------------------------
// 计算直线拟合的斜率
    double calculateSlope(const std::vector<Point>& points);
    // 判断点云是否在一条直线上
    bool isLine(const std::vector<Point>& points);
    // 从points中取出中间一个数据，主要针对反光板
    Point getMiddlePoint(const std::vector<Point>& points);
    //设置反光柱标记并发布 //设置标记尺寸 0.09 0.09 0.50
    void set_current_marker_property(const double mx,const double my,double width, double height,double length,int n,std::string & frame_id,double & a,double & r,double & g,double &b,std::string &ns_id,double &lifetime);
    //苏凯 todo 设置反光板匹配标记并发布  length  0.001厚度设为一个很小的值，表示一个平面
    void set_current_light_panel_property(const double lx,const double ly, double width, double height,double length,  int& markerId,std::string & frame_id,double & a,double & r,double & g,double &b,std::string &ns_id,double &lifetime);
    //文本标记
    void Publish(ros::Publisher  &pub_marker,visualization_msgs::Marker &marker);
    //用于发布文本标记。
    void publishTextMarker(const double x, const double y,std::string & frame_id,ros::Publisher & marker_pub,std::string& datatxt,int &markerid,
                           double & a,double & r,double & g,double &b,std::string &ns_id,double &lifetime);

    std::vector<double> FirstBezier(std::vector<double> ps, std::vector<double> pe, double t);

    friend std::ostream& printPoint(std::ostream& os,const std::shared_ptr<ReflectorPanelPoint> &point) {
        //打印数据
//    std::cout << "ReflectorPanelPoint shared_point1 :\n";
//    printPoint(std::cout, shared_point1);
//    std::cout << std::endl;
        os << "ReflectorPanelPoint =》 "
           << ", ID: " << point->id << ", Shape: " << point->shape
           << ", Range: " << point->range << ", Intensity: " << point->intensity
           << ", source_frame_id: " << point->source_frame_id
           << ", source_Position: (" << point->x << ", " << point->y << ", " << point->z << ")"
           << ", source_Orientation: (" << point->rx << ", " << point->ry << ", " << point->rz << ", " << point->rw << ")"
           << ", target_frame_id: " << point->target_frame_id
           << ", target_Position: (" << point->scan_x << ", " << point->scan_y << ", " << point->scan_z << ")"
           << ", target_Orientation: (" << point->scan_rx << ", " << point->scan_ry << ", " << point->scan_rz << ", " << point->scan_rw << ")"
           << ", Angle: " << point->angle << ", Distances: " << point->distances;
        return os;
    }

//直接用 cout<<point<<打印
    friend std::ostream& operator<<(std::ostream& os,const std::shared_ptr<ReflectorPanelPoint> &point) {
        os << "ReflectorPanelPoint =》 "
           << ", ID: " << point->id << ", Shape: " << point->shape
           << ", Range: " << point->range << ", Intensity: " << point->intensity
           << ", source_frame_id: " << point->source_frame_id
           << ", source_Position: (" << point->x << ", " << point->y << ", " << point->z << ")"
           << ", source_Orientation: (" << point->rx << ", " << point->ry << ", " << point->rz << ", " << point->rw << ")"
           << ", target_frame_id: " << point->target_frame_id
           << ", target_Position: (" << point->scan_x << ", " << point->scan_y << ", " << point->scan_z << ")"
           << ", target_Orientation: (" << point->scan_rx << ", " << point->scan_ry << ", " << point->scan_rz << ", " << point->scan_rw << ")"
           << ", Angle: " << point->angle << ", Distances: " << point->distances;
        return os;
    }

};

#endif // REFLECTOR_PANEL_POINT_H
//--------------------------------------------------------------------------------------

// 在 .cpp 文件中定义静态成员变量
std::unordered_map<std::string, std::shared_ptr<ReflectorPanelPoint>> ReflectorPanelPoint::instances;
std::mutex ReflectorPanelPoint::instance_mutex;

void ReflectorPanelPoint::initparam() {
    // 将平移部分设置为 (0, 0, 0)
    tf::Vector3 zero_translation(0.0, 0.0, 0.0);

    // 将旋转部分设置为单位四元数 (0, 0, 0, 1)
    tf::Quaternion identity_rotation(0.0, 0.0, 0.0, 1.0);

    // 对 last_laser_to_odom_transform_ 赋值
    last_laser_to_odom_transform_.setOrigin(zero_translation);
    last_laser_to_odom_transform_.setRotation(identity_rotation);

    // 同样对 last_mapTbase_link_transform_ 赋值
    last_mapTbase_link_transform_.setOrigin(zero_translation);
    last_mapTbase_link_transform_.setRotation(identity_rotation);

    // 设置时间戳和坐标系ID（可选，根据你的实际需要设置）
    ros::Time current_time = ros::Time::now();
    last_laser_to_odom_transform_.stamp_ = current_time;
    last_mapTbase_link_transform_.stamp_ = current_time;
    //这个变换描述了 laser 坐标系 在 odom 坐标系 中的位置和姿态。
    last_laser_to_odom_transform_.frame_id_ = odom_frame_id_;
    last_laser_to_odom_transform_.child_frame_id_ = laser_frame_id_;
    //这个变换描述了 base_link 坐标系 在 map 坐标系 中的位置和姿态。
    last_mapTbase_link_transform_.frame_id_ = map_frame_id_;
    last_mapTbase_link_transform_.child_frame_id_ = base_link_frame_id_;

    ROS_INFO("Transforms initialized to identity matrices.");
}
ReflectorPanelPoint::ReflectorPanelPoint() {
    initparam();
}



ReflectorPanelPoint::ReflectorPanelPoint(const std::string& id): id(id) {
    initparam();

}
ReflectorPanelPoint::ReflectorPanelPoint(const std::string& id,int marker_id, double x, double y, double z, double rx, double ry, double rz, double rw):id(id),reflectorPanelPoint_marker_id_(marker_id), x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), rw(rw) {
    initparam();
}

ReflectorPanelPoint::ReflectorPanelPoint(const std::string& id,int marker_id, double x, double y, double z, double rx, double ry, double rz, double rw,string &shape,int &reflectorPanelPoint_text_id):id(id),reflectorPanelPoint_marker_id_(marker_id), x(x), y(y), z(z), rx(rx), ry(ry), rz(rz), rw(rw), shape(shape) , reflectorPanelPoint_text_id_(reflectorPanelPoint_text_id){
    initparam();
}
//-----------------------------------------------------------------


// 删除数据
bool ReflectorPanelPoint::deleteAllInstance() {

    for (auto it = ReflectorPanelPoint::instances.begin(); it != ReflectorPanelPoint::instances.end(); ++it) {
        const std::string& key = it->first;
        const std::shared_ptr<ReflectorPanelPoint>& panelPoint = it->second;
        panelPoint->isMatchok_=-1;//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
        panelPoint->currentIsMatchok_=-1;//-1：未开始匹配  0： laserTscan_x 已经被使用了，laserTscan_x这数据需只能通过tf或odom的变化量来预测更新成为最新的数据  1:匹配上并给雷达 laserTscan_x 数据赋值了

        panelPoint->stopTransformThread();
        panelPoint->stopTransformThread2();
    }
    instances.clear();
    return true;
}
// 删除数据
bool ReflectorPanelPoint::deleteInstance(const std::string& id) {
    auto it = instances.find(id);
    if (it != instances.end()) {
        it->second->isMatchok_=-1;//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
        it->second->currentIsMatchok_=-1;//-1：未开始匹配  0： laserTscan_x 已经被使用了，laserTscan_x这数据需只能通过tf或odom的变化量来预测更新成为最新的数据  1:匹配上并给雷达 laserTscan_x 数据赋值了
        it->second->stopTransformThread();
        it->second->stopTransformThread2();
        // 由于std::shared_ptr管理对象的生命周期，当最后一个引用被删除时，对象会被自动销毁。所以您无需担心内存泄漏问题。
        // delete it->second;
        instances.erase(it);
        return true;
    } else {
        return false;
    }
}


//初始化数据
void ReflectorPanelPoint::initInstance() {
    for (auto& pair : instances) {
        if (pair.second && pair.second->isMatchok_ != -1) {
            pair.second->isMatchok_ = -1;//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
            pair.second->currentIsMatchok_ = -1;
        }
    }
}

// 查找对象
bool ReflectorPanelPoint::getInstance(const std::string& id,std::shared_ptr<ReflectorPanelPoint> & reflectorPanelPoint) {

    auto it = instances.find(id);
    if (it == instances.end()) {
        return false;
    }
    reflectorPanelPoint = instances[id];
    return true;
}

// 初始化静态成员,加载json文件时会用到
std::shared_ptr<ReflectorPanelPoint> ReflectorPanelPoint::createInstance1(const std::string& id) {
    std::lock_guard<std::mutex> lock(instance_mutex);
    auto it = instances.find(id);
    if (it == instances.end()) {
        auto new_point = std::make_shared<ReflectorPanelPoint>(id);
        instances[id] = new_point;
        instances[id]-> careatrRosNode();
        instances[id]->startTransformThread();
        instances[id]->startTransformThread2();//时时更新雷达扫描到的反光板位置
        new_point->last_seen_=ros::Time::now();//初始化时间
        return new_point;
    }
    return it->second;
}



// 初始化静态成员，地图选取时会用到
std::shared_ptr<ReflectorPanelPoint> ReflectorPanelPoint::createInstance3(const std::string& id,int marker_id, double x, double y, double z,double  rx, double ry, double rz, double rw,string &shape,int &reflectorPanelPoint_text_id) {

    auto it = instances.find(id);
    if (it == instances.end()) {
        std::lock_guard<std::mutex> lock(instance_mutex);//互锁
        //td::shared_ptr<ReflectorPanelPoint>
        auto  new_point = std::make_shared<ReflectorPanelPoint>(id,marker_id,  x,  y,  z,  rx,  ry,  rz,  rw,shape,reflectorPanelPoint_text_id);
        instances[id] = new_point;
        instances[id]-> careatrRosNode();
        instances[id]->startTransformThread();//base_link 到扫描到的雷达的距离 distances
        instances[id]->startTransformThread2();//时时更新雷达扫描到的反光板位置
        //    std::string shape="CYLINDER";//CUBE：立方体（平面）,CYLINDER: 圆柱体

        new_point->last_seen_=ros::Time::now();//初始化时间


        return new_point;
    }
    return it->second;
}



// 初始化静态成员，地图选取时会用到
std::shared_ptr<ReflectorPanelPoint> ReflectorPanelPoint::createInstance2(const std::string& id,int marker_id, double x, double y, double z,double  rx, double ry, double rz, double rw) {

    auto it = instances.find(id);
    if (it == instances.end()) {
        std::lock_guard<std::mutex> lock(instance_mutex);//互锁
        auto new_point = std::make_shared<ReflectorPanelPoint>(id,marker_id,  x,  y,  z,  rx,  ry,  rz,  rw);
        instances[id] = new_point;
        instances[id]-> careatrRosNode();
        instances[id]->startTransformThread();//base_link 到扫描到的雷达的距离 distances
        instances[id]->startTransformThread2();//时时更新雷达扫描到的反光板位置
        //    std::string shape="CYLINDER";//CUBE：立方体（平面）,CYLINDER: 圆柱体

        new_point->last_seen_=ros::Time::now();//初始化时间


        return new_point;
    }
    return it->second;
}




//=====================多线程1====================================
//时时计算 base_link 到扫描到的雷达的距离 distances 。
//开启多线程
void ReflectorPanelPoint::startTransformThread() {
    if (!transform_thread.joinable()) {
        transform_thread = std::thread(&ReflectorPanelPoint::transformThreadFunc, this);
    }
}
//停止多线程
void ReflectorPanelPoint::stopTransformThread() {
    if (transform_thread.joinable()) {
        should_stop.store(true);
        transform_thread.join();
    }
}
//多线程启动获取tf数据
void ReflectorPanelPoint::transformThreadFunc() {
    int time_milliseconds=10;//1000
    while (!should_stop.load()) {
        {
            if(!iswileTure_){
                return;
            }


            transformAndCalculate();

            transformAndCalculate2();

        }
    }
}

std::vector<double> ReflectorPanelPoint::FirstBezier(std::vector<double> ps, std::vector<double> pe, double t)
// vector<double> GlobalPlanner::FirstBezier(vector<double> pe, vector<double> ps, double t)
{
    std::vector<double> point;
    point.push_back((1-t)*ps[0]+t*pe[0]);
    point.push_back((1-t)*ps[1]+t*pe[1]);

    return point;
}

//多线程订阅tf数据
void ReflectorPanelPoint::transformAndCalculate() {
    try {

        // 尝试非阻塞上锁
        std::unique_lock<std::mutex> lock(reflectorPanelPoint_transformAndCalculate_mutex_, std::try_to_lock);
        if (!lock.owns_lock()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(50));//todo 不延时会加载不到数据

            return;
        }



        double current_baselinkTscan_x_1=current_baselinkTscan_x;
        double current_baselinkTscan_y_1=current_baselinkTscan_y;
        double current_baselinkTscan_z_1=current_baselinkTscan_z;
        double current_baselinkTscan_rx_1=current_baselinkTscan_rx;
        double current_baselinkTscan_ry_1=current_baselinkTscan_ry;
        double current_baselinkTscan_rz_1=current_baselinkTscan_rz;
        double current_baselinkTscan_rw_1=current_baselinkTscan_rw;
        double base_linkTfanguangban_distances_1 =  base_linkTfanguangban_distances;



        double lifetime=0.05;//延时时间
        //  double  a=1.0,r=1.0,g=1.0,b=1.0;//3034 有颜色demo
        //todo 2. base_linkTfanguangban （距离太短，会计算 yaw 不稳定）
        //todo 设置反光板匹配标记并发布
        if(isShowMatchokReflectorFanGuanBanPoint_){

            if(isMatchok_==1) //1:匹配上了 绿色
            {

                //todo 1. 上一次 通过雷达探测到的实时 odom坐标系下的反光柱坐标
                tf::Transform previous_odomTscanpoint_transform;
                previous_odomTscanpoint_transform.setOrigin(tf::Vector3(circle.odomTscan_x, circle.odomTscan_y, circle.odomTscan_z ));
                //tf::Quaternion q;
                tf::Quaternion previous_odomTscanpoint_q(circle.odomTscan_rx, circle.odomTscan_ry, circle.odomTscan_rz, circle.odomTscan_rw); //  x, y, z，w, 分量
                previous_odomTscanpoint_transform.setRotation(previous_odomTscanpoint_q);//base_link_to_odom_transform;//odomTbase_link

                tf::Transform  laserTscanPoint_now = now_odomTlaser_transform_.inverse() * previous_odomTscanpoint_transform;

                tf::Transform  base_linkTfanguangban_transform  = laser_to_base_link_transform_ * laserTscanPoint_now;
                //base_linkTfanguangban
                double base_linkTfanguangban_to_map_x=  base_linkTfanguangban_transform.getOrigin().getX();
                double base_linkTfanguangban_to_map_y=  base_linkTfanguangban_transform.getOrigin().getY();
                double base_linkTfanguangban_to_map_z=  base_linkTfanguangban_transform.getOrigin().getZ();

                //scan_CB_new1 当前时时扫描到的反光柱在baselink下的坐标 【通过这个值可以求移动机器人的姿态或者，全局反光柱的当前姿态】

                current_baselinkTscan_x_1=base_linkTfanguangban_to_map_x;
                current_baselinkTscan_y_1=base_linkTfanguangban_to_map_y;
                current_baselinkTscan_z_1=base_linkTfanguangban_to_map_z;
                current_baselinkTscan_rx_1=0.0;
                current_baselinkTscan_ry_1=0.0;
                current_baselinkTscan_rz_1=0.0;
                current_baselinkTscan_rw_1=1.0;
                base_linkTfanguangban_distances_1 =  std::sqrt(std::pow(base_linkTfanguangban_to_map_x , 2) + std::pow(base_linkTfanguangban_to_map_y, 2) );


                current_baselinkTscan_x=base_linkTfanguangban_to_map_x;
                current_baselinkTscan_y=base_linkTfanguangban_to_map_y;
                current_baselinkTscan_z=base_linkTfanguangban_to_map_z;
                current_baselinkTscan_rx=0.0;
                current_baselinkTscan_ry=0.0;
                current_baselinkTscan_rz=0.0;
                current_baselinkTscan_rw=1.0;
                base_linkTfanguangban_distances =  base_linkTfanguangban_distances_1;

                //  a=1.0,r=0.0,g=1.0,b=0.0;//3034  有颜色demo
//                    // /reflectormarker_marker
                set_current_marker_property(current_baselinkTscan_x,current_baselinkTscan_y,fangaungzhuWidth_,fangaungzhuHeight_,fangaungzhuLength_,reflectorPanelPoint_marker_id_,base_link_frame_id_, a_,r_,g_,b_,id,lifetime);
                double text_x=current_baselinkTscan_x+fangaungzhuWidth_,text_y= current_baselinkTscan_y+fangaungzhuWidth_;
                std::this_thread::sleep_for(std::chrono::milliseconds(5));//todo 不延时会加载不到数据
                //发布文字 reflectorPanelPoint_text_id_
                publishTextMarker(text_x, text_y,base_link_frame_id_, pub_reflectormarker, id, reflectorPanelPoint_text_id_, a_,r_,g_,b_,id,lifetime);

            }else{
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
            }
        }// if(isShowMatchokReflectorFanGuanBanPoint_){
        else{
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

        //发布匹配上的反光板连线及预测到的连线,isShowFanGuanBanPoint_:看调试效果
        // if(isShowFanGuanBanmarkerPath_&&isMatchok_==1){
        if(isShowFanGuanBanmarkerPath_){// -1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点

            visualization_msgs::Marker line_marker;
            //  line_marker.header.frame_id =laser_frame_id_; // 设置参考坐标系
            line_marker.header.frame_id =base_link_frame_id_; // 设置参考坐标系
            line_marker.header.stamp = ros::Time::now(); // 设置时间戳
            line_marker.ns = id; // 命名空间，用于区分不同的Marker
            line_marker.id = reflectorPanelPoint_marker_id_; // Marker的ID
            line_marker.type = visualization_msgs::Marker::LINE_STRIP; // 设置Marker类型为线段

            line_marker.action = visualization_msgs::Marker::ADD; // Marker的操作类型：添加
            line_marker.lifetime = ros::Duration(lifetime); // Marker持续时间，0表示直到清除命令为止

            if(isMatchok_==1) //1:匹配上了 绿色
            {


                line_marker.color.r = r_;
                line_marker.color.g = g_;
                line_marker.color.b = b_;
                line_marker.color.a = a_; // alpha透明度

                line_marker.scale.x = 0.01; // 线宽
                // 添加线段的起点和终点
                geometry_msgs::Point start_point, end_point;
                start_point.x = 0.0;
                start_point.y = 0.0;
                start_point.z = 0.0;

                end_point.x = current_baselinkTscan_x_1;//基于扫描匹配到的反光柱
                end_point.y = current_baselinkTscan_y_1;

                end_point.z = 0.0;
                line_marker.points.push_back(start_point); // 添加起点
                line_marker.points.push_back(end_point); // 添加终点
                path_marker_pub.publish(line_marker); // 发布Marker消息
                // std::this_thread::sleep_for(std::chrono::milliseconds(20));//todo 不延时会加载不到数据

            }else{
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
            }


        }else{
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));//todo 不延时会加载不到数据

    } catch (tf::TransformException& ex) {
        ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());
    }
}

//---------------------多线程2---------------------------------reflectorPanelPoint_marker_id_-------------------
// 时时更新雷达扫描到的反光板位置
//开启多线程2
void ReflectorPanelPoint::startTransformThread2() {
    if (!transform_thread2.joinable()) {
        transform_thread2 = std::thread(&ReflectorPanelPoint::transformThreadFunc2, this);
    }
}
//停止多线程2
void ReflectorPanelPoint::stopTransformThread2() {
    if (transform_thread2.joinable()) {
        should_stop2.store(true);
        transform_thread2.join();
    }
}
//多线程启动获取tf数据2
void ReflectorPanelPoint::transformThreadFunc2() {
//    int time_milliseconds=20;//2000  20
//    while (!should_stop2.load()) {
//        {
//            if(!iswileTure_){
//                return;
//            }
//
//            transformAndCalculate2();
//            // 根据需要添加适当的延时，例如 10ms
//            std::this_thread::sleep_for(std::chrono::milliseconds(time_milliseconds));
//
//        }
//    }// while (!should_stop2.load())
}


void ReflectorPanelPoint::transformAndCalculate2() {

    try {

        std::unique_lock<std::mutex> lock(reflectorPanelPoint_transformAndCalculate_mutex2_, std::try_to_lock);
        if (!lock.owns_lock()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            return;
        }



        if(fabs(laserTscan_x)+ fabs(laserTscan_y)<=0.0000001){
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            return;
        }


        // 反光柱在 map下的姿态是比较重要的，求不出来会影响单个反光柱计算定位问题，主要计算在3239行 trilateration_with_yaw_new
        //todo 1. mapTscanpoint
        tf::Transform mapTscanpoint_transform; //mapTscanpoint_transform
        mapTscanpoint_transform.setOrigin(tf::Vector3(x, y, z));
        //tf::Quaternion q;
        //tf::Quaternion q2(rx, ry, rz, rw); //  x, y, z，w, 分量
        tf::Quaternion q2(0.0, 0.0, 0.0, 1.0); //  x, y, z，w, 分量
        // q.setRPY(0, 0, reflectorPanelPoint->laserTscan_yaw);
        mapTscanpoint_transform.setRotation(q2);//base_link_to_odom_transform;//odomTbase_link





        //todo  计算部署在环境中的反光板在 base_link 下的坐标， [涉及到了map，所以这个距离是依赖先有准确base_link定位才行]
        if(fabs(x)+ fabs(y)>0.0001&&isMatchok_!=-1){

            //  mapTscanpoint = mapTbaselink * baselinkTscanPoint
            //  baselinkTscanPoint = mapTbaselink.inverse() * mapTscanpoint
            //   tf::Transform  base_linkTscanpoint=  mapTbase_link_transform.inverse()*mapTscanpoint_transform;
            tf::Transform  base_linkTscanpoint=  base_link_to_map_transform_.inverse()*mapTscanpoint_transform;


            // 部署在环境中的反光板在 base_link 下的坐标
            // base_linkTscanPoin  base_linkTfanguangban t  把’在map坐标系下的雷达数‘ x=0.0, y=0.0, z=0.0。 换算到laser坐标系下 scan_x=0.0, scan_y=0.0, scan_z=0.0。
            // 这样就可以跟踪到 scan_x 离雷达最近，且scan_x>0的数据=》永远在雷达的正方向，安排序规则来排序 scan_x>=0且值最小。
            scan_x=base_linkTscanpoint.getOrigin().getX();
            scan_y=base_linkTscanpoint.getOrigin().getY();
            scan_z=base_linkTscanpoint.getOrigin().getZ();
            scan_rx=base_linkTscanpoint.getRotation().getX();
            scan_ry=base_linkTscanpoint.getRotation().getY();
            scan_rz=base_linkTscanpoint.getRotation().getZ();
            scan_rw=base_linkTscanpoint.getRotation().getW();
            scan_yaw=tf::getYaw(base_linkTscanpoint.getRotation());

        }

        //--------------------------------------------------------------------
        // todo 反光板到 base_link 的距离 [通过坐标变换计算的到的距离，涉及到了map，所以这个距离是依赖先有准确base_link定位才行]
        // todo  这个 distances 与  scan_x 在 集合 reflectorPanelPoints_vector 自定义排序中使用到的【在搜索匹配中可以用到，数据比较多的情况下可以搜索集合最前面的几条数据，提高搜索效率】， 自定义比较器 CompareByDistanceAndX 1436行。


        double base_link_to_map_x= base_link_to_map_transform_.getOrigin().getX();
        double base_link_to_map_y=  base_link_to_map_transform_.getOrigin().getY();
        double  fanguangban_to_map_x=  mapTscanpoint_transform.getOrigin().getX();//base_linkTfanguangban
        double fanguangban_to_map_y=  mapTscanpoint_transform.getOrigin().getY();
        //部署的 反光板 到 base_link 的距离 [通过坐标变换计算的到的距离，涉及到了map，所以这个距离是依赖先有准确定位才行]
        distances =  std::sqrt(std::pow(base_link_to_map_x-fanguangban_to_map_x , 2) + std::pow(base_link_to_map_y-fanguangban_to_map_y, 2) );
        //  std::cout<<id<<" : ----到某固定点的距离d----690-------distances-------------------d1: "<<distances <<std::endl;
        //=========================================================================================================================================

        // std::this_thread::sleep_for(std::chrono::milliseconds(10));
    } catch (tf::TransformException& ex) {

        ROS_WARN("%s", ex.what());

    }

}


//----------------------------------------------------------------------





// 在类中增加订阅函数
void ReflectorPanelPoint::careatrRosNode() {//ros::NodeHandle& nh
    // 订阅名为"odom"的话题，回调函数为odomCallback
    // odom_sub_ =nh.subscribe("/odom", 10, &ReflectorPanelPoint::odomCallback, this);
    pub_reflectormarker = nh_.advertise<visualization_msgs::Marker>(reflectormarker_marker_toptic_name_, 10);
    path_marker_pub = nh_.advertise<visualization_msgs::Marker>(reflectormarker_path_marker_toptic_name_, 10);  //  scan_sub_ = nh.subscribe<sensor_msgs::LaserScan>("scan", 1000, boost::bind(&ReflectorPanelPoint::scanCallback, this, _1));

}
//odom
void ReflectorPanelPoint::odomCallback(const nav_msgs::Odometry::ConstPtr &msg){

}
//发布标记
void ReflectorPanelPoint::Publish(ros::Publisher  &pub_marker,visualization_msgs::Marker &marker){

//    while (pub_marker.getNumSubscribers() < 1) {//todo 判断订阅的对象是否创建
//        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
//        continue;
//    }
    // if(pub_marker.getNumSubscribers() >0){
    marker.header.stamp = ros::Time::now();
    pub_marker.publish(marker);
    // }

}


//用于发布文本标记。
void ReflectorPanelPoint::publishTextMarker(const double x,const double y,std::string &frame_id,ros::Publisher & marker_pub,std::string& datatxt,int &markerid,
                                            double & a,double & r,double & g,double &b,std::string &ns_id,double &lifetime) {
    visualization_msgs::Marker text_marker;
    text_marker.header.frame_id = frame_id;
    text_marker.header.stamp = ros::Time::now();
    text_marker.ns =ns_id ;//"target_tracking"
    text_marker.id = markerid;
    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;  //int 类型
    text_marker.action = visualization_msgs::Marker::ADD;

    text_marker.pose.position.x = x;
    text_marker.pose.position.y = y;
    text_marker.pose.position.z = 1.0;  // 高度稍微高一点，使文本可见
    text_marker.pose.orientation.w = 1.0;

    text_marker.scale.z = 0.16;//0.2  // 文本大小
    text_marker.color.a = a;//1.0
    text_marker.color.r = r;//1.0
    text_marker.color.g = g;//1.0
    text_marker.color.b = b;//1.0

    text_marker.text = datatxt;
    text_marker.lifetime = ros::Duration(lifetime); //0.5s后消失，输入0时为不消失
    marker_pub.publish(text_marker);
}

//设置反光柱标记并发布
void  ReflectorPanelPoint::set_current_marker_property(double mx,double my,double width, double height,double length,int n,std::string & frame_id,double & a,double & r,double & g,double &b,std::string &ns_id,double &lifetime){


    visualization_msgs::Marker marker;
    // 决定从哪个视图可以看到标记
    marker.header.frame_id = frame_id;//todo sukai 雷达的frame_id要改为laser_frame
    marker.ns =ns_id ;//"sign"
    marker.id = n;
    //设置标记类型：圆柱体
    marker.type = visualization_msgs::Marker::CYLINDER;//int 类型

    //设置标记坐标
    marker.pose.position.x = mx;
    marker.pose.position.y = my;
    marker.pose.position.z = 0.8;

    //设置标记尺寸 0.09 0.09 0.50
    marker.scale.x = width;  //m
    marker.scale.y = height;
    marker.scale.z = length;

    //设置标记颜色
    marker.color.a = a; // Don't forget to set the alpha! 1.0 1.0 1.0 1.0
    marker.color.r = r;
    marker.color.g = g;
    marker.color.b = b;

    //设置标记动作
    marker.action = visualization_msgs::Marker::ADD;
    // marker.lifetime = ros::Duration(1.0); //0.5s后消失，输入0时为不消失
    marker.lifetime = ros::Duration(lifetime); //0.5s后消失，输入0时为不消失
    Publish(pub_reflectormarker,marker); // 发布 /reflectormarker_marker
}

//苏凯 todo 设置反光板匹配标记并发布
void  ReflectorPanelPoint::set_current_light_panel_property(const double lx,const double ly, double width, double height,double length, int &markerId,std::string & frame_id,double & a,double & r,double & g,double &b,std::string &ns_id,double &lifetime) {
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;//todo sukai 雷达的frame_id要改为laser_frame

    //    决定从哪个视图可以看到光板
    marker.ns =ns_id;//"light_panel"
    marker.id = markerId;
    // 设置标记类型：立方体（平面）
    marker.type = visualization_msgs::Marker::CUBE;//int 类型

    // 设置标记坐标
    marker.pose.position.x = lx;
    marker.pose.position.y = ly;
    marker.pose.position.z = 0;

    // 设置标记尺寸
    marker.scale.x = width;  // 宽度
    marker.scale.y = height; // 高度
    marker.scale.z = length;  // 0.001厚度设为一个很小的值，表示一个平面

    // 设置标记颜色
    marker.color.a = a; // 透明度设为0.5，让光板半透明, 0.5 1.0 1.publishTextMarker0 0.0
    marker.color.r = r;
    marker.color.g = g;
    marker.color.b = b; // 黄色，你可以根据需要修改颜色

    // 设置标记动作
    marker.action = visualization_msgs::Marker::ADD;
    //  marker.lifetime = ros::Duration(1.0); // 1秒后消失，输入0时为不消失
    marker.lifetime = ros::Duration(lifetime); //0.5s后消失，输入0时为不消失
    Publish(pub_reflectormarker,marker); // 发布 /reflectormarker_marker
}

//===================================================



//通过反光柱计算得到的定位数据
struct OptimizePoseWithTrilaterationPose {
    ros::Time stamp;
    tf::StampedTransform last_laser_to_odom_transform;//  odomTlaser 记录匹配上时的历程数据
    tf::StampedTransform  last_mapTlaser_transform;//记录匹配上时的历程数据
    tf::StampedTransform  last_odomTbase_link_transform;
    tf::Transform   mapTbase_transform_min ;//通过反光柱计算得到的定位数据
    std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> > indices;
    std::vector<Point> circle;
    string matchReflectorPaneIds="EMPTY";
};

std::deque<OptimizePoseWithTrilaterationPose> optimizePoseWithTrilaterationPose_queue_;

//======================================================================================================
//-------------------------------------------------------------

//todo 匹配上的反光板,开线程用经典三边定位最小二乘法计算坐标，并发布tf
std::queue<std::vector<std::shared_ptr<ReflectorPanelPoint>>> matchokReflectorPanelPoint_;
//三边定位最小二乘法计算坐标完成的点，放这里进行卡尔曼滤波跟踪。匹配成功点会从 matchokReflectorPanelPoint_ 备份方在这里，给卡尔曼滤波算法使用
//std::queue<std::vector<std::shared_ptr<ReflectorPanelPoint>>> matchokReflectorPanelPointEnd_;
// 队列和线程同步工具 开一个线程来 处理这matchokReflectorPanelPoint_里面的数据
std::mutex queue_mutex_;
std::mutex scancallback_mutex_;
std::condition_variable queue_cond_var_;
bool stop_thread_ = false;
//-------------------------------------
std::mutex sortSetPeriodically_mutex_;
std::condition_variable sortSetPeriodically_cond_var_;
//--------------------------------------------------

//================================================自定义哈希函数===std::vector<std::string> key ========================================================

// 自定义哈希函数和相等比较器
struct VectorStringHash {
    std::size_t operator()(const std::vector<std::string>& vec) const {
        std::size_t hash = 0;
        for (const auto& str : vec) {
            hash ^= std::hash<std::string>()(str) + 0x9e3779b9 + (hash << 6) + (hash >> 2);
        }
        return hash;
    }
};

struct VectorStringEqual {
    bool operator()(const std::vector<std::string>& lhs, const std::vector<std::string>& rhs) const {
        return lhs.size() == rhs.size() && std::equal(lhs.begin(), lhs.end(), rhs.begin());
    }
};
//-------------------------------------std::tuple<double, double> key--------------------------------------------------------------------------

// 定义哈希函数
struct TupleHash {
    std::size_t operator()(const std::tuple<double, double>& key) const {
        auto roundedVal1 = round(std::get<0>(key));
        auto roundedVal2 = round(std::get<1>(key));
        auto h1 = std::hash<decltype(roundedVal1)>{}(roundedVal1);
        auto h2 = std::hash<decltype(roundedVal2)>{}(roundedVal2);
        return h1 ^ (h2 << 1); // XOR and bit shift

    }
};

// 定义相等比较函数  209，  epsilon_  容忍阀值
struct TupleEqual {
    bool operator()(const std::tuple<double, double>& lhs, const std::tuple<double, double>& rhs) const {
        return std::fabs(std::get<0>(lhs) - std::get<0>(rhs)) < epsilon_ &&
               std::fabs(std::get<1>(lhs) - std::get<1>(rhs)) < epsilon_;
    }
};

// 自定义比较器
struct CompareByDistanceAndX {
    bool operator()(const std::shared_ptr<ReflectorPanelPoint>& a, const std::shared_ptr<ReflectorPanelPoint>& b) const {

        if (a->scan_x > 0 && b->scan_x > 0) {

            return a->distances < b->distances;
        } else if (a->scan_x > 0 && b->scan_x <= 0) {

            return true;
        } else if (a->scan_x <= 0 && b->scan_x > 0) {

            return false;
        } else {

            return a->distances < b->distances;
        }
    }
};


//auto foundPoint = findPoint(reflectorPanelPoints_vector_,transformed_circle[i].x, transformed_circle[i].y, epsilon_);
std::shared_ptr<ReflectorPanelPoint> findPoint(const std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> &reflectorPanelPoints_vector,double targetX, double targetY, double epsilon) {

    for (const auto& point : reflectorPanelPoints_vector) {
        if (std::fabs(point->x - targetX) <= epsilon && std::fabs(point->y - targetY) <= epsilon) {
            return point;
        }
    }
    return nullptr;
}


std::shared_ptr<ReflectorPanelPoint> findPointByHalf(const std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> &reflectorPanelPoints_vector,double targetX, double targetY, double epsilon) {
    auto it = reflectorPanelPoints_vector.begin();
    auto midpoint = std::next(it, reflectorPanelPoints_vector.size() / reflectorPanelPoints_vector_size_);
    for (; it != midpoint; ++it) {
        const auto& point = *it;
        if (point->isMatchok_!=2) //非预测的数据直接不要 ，-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测到下一时刻的需匹配反光板
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            continue;
        }
        if (std::fabs(point->x - targetX) <= epsilon && std::fabs(point->y - targetY) <= epsilon) {
            return point;
        }
    }
    return nullptr; // 如果没有找到，返回 nullptr
}

//或其鼠标点击rviz获取反光板坐标
std::map<std::string,Point> pointsSaveMap_;//或其鼠标点击rviz获取反光板坐标
//---------------------------------------------------------------------------
//todo  std::map 储存所有反光板数据    map <id,ReflectorPanelPoint>
std::map<std::string ,std::shared_ptr<ReflectorPanelPoint>> reflectorPanelPointAllMaps_;


//--------------------------------------------------------------------------------------------
//todo std::vector 储存所有 排序好的反光板数据 排序规则为 反光板点位里小车中心坐标比较最近的在前面且x坐标是正值的数据。如果x坐标是负值说明反光板在小车的后面，理论上以经匹配过了不需要排在前面
std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> reflectorPanelPoints_vector_;
//todo 储存所有的点为 std::tuple<double, double> 为 key  ， map下的反光板全局坐标 std::tuple<x, y> 为key
std::unordered_map<std::tuple<double, double>, std::shared_ptr<ReflectorPanelPoint>, TupleHash, TupleEqual>  reflectorPanelPointAllUnordered_maps_;
//------------------------------------------
//todo 储存所有有计算好边长的数据 1个key
std::unordered_map<std::string , std::vector<std::tuple<std::vector<double>, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>>> unorderedEdgeMapNew_;
//  2个key   std::vector<std::string> key = {"ID25", "ID9"};
std::unordered_map<std::vector<std::string>, std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>,VectorStringHash, VectorStringEqual> unorderedEdgeMapNew_IdKey_;
//--------------------------------------------
//todo 储存匹配上的点位  map下的反光板全局坐标 std::tuple<x, y> 为key
std::unordered_map<std::tuple<double, double>, std::shared_ptr<ReflectorPanelPoint>, TupleHash, TupleEqual>  matchokReflectorPanelPointEnd_Unordered_map_;

//===================================================================================================================





void sortSetPeriodically(std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX>& reflectorSet, int intervalSeconds) {
    std::cout << "reflectorPanelPoints_vector_ 定时排序函数" << std::endl;

    while (iswileTure_) {
        if(!iswileTure_){
            return;
        }
        {
            if(!is_trilateration_){  // 停止排序
                continue;
            }

            if(!iswirtortSetPeriodically_){  // 停止排序
                continue;
            }

            std::unique_lock<std::mutex> lock(sortSetPeriodically_mutex_);
            //条件变量通常与一个互斥锁（例如std::mutex）结合使用，以保护共享资源，并允许线程以线程安全的方式等待某个条件满足。
            // sortSetPeriodically_cond_var.wait(lock, [] {  return  stop_thread_|| reflectorSet.size()==0; });
            //todo 苏凯 有阻塞现象
//            sortSetPeriodically_cond_var_.wait(lock, [] { return stop_thread_ ; });
//            if (stop_thread_ ) {
//                //  lock.unlock();
//                continue;
//            }


            // intervalSeconds 1000
            // std::this_thread::sleep_for(std::chrono::seconds(intervalSeconds)); // 等待一段时间
            std::this_thread::sleep_for(std::chrono::milliseconds(intervalSeconds));
            if(reflectorSet.size()==0)
                continue;

            reflectorSet = std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX>(reflectorSet.begin(), reflectorSet.end());


        }



    }
}

//=======================================================


//======================================================================================================


// 比较边长是否相等的函数
bool areEdgesEqualNew(double edge1, double edge2, double threshold) {
    return std::abs(edge1 - edge2) <= threshold;
}


//对比角度是否一致
bool areAnglesEqualNew(double angle1, double angle2, double threshold) {
    return std::fabs(angle1 - angle2) < threshold;
}



// 计算两点间的欧几里得距离
double calculateDistance(const ReflectorPanelPoint& point1, const ReflectorPanelPoint& point2) {
    double dx = point1.x - point2.x;
    double dy = point1.y - point2.y;
    double dz = point1.z - point2.z;
    return std::sqrt(dx*dx + dy*dy + dz*dz);
}


// 计算两点之间的距离
double calculateDistance(const Point& p1, const Point& p2) {
    return std::sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y));
}


// 计算三点之间的夹角（返回值是弧度）
double calculateAngle(const Point& p1, const Point& p2, const Point& p3) {

    double v1x = p2.x - p1.x;
    double v1y = p2.y - p1.y;
    double v2x = p3.x - p1.x;
    double v2y = p3.y - p1.y;


    double dotProduct = v1x * v2x + v1y * v2y;
    double length1 = std::sqrt(v1x * v1x + v1y * v1y);
    double length2 = std::sqrt(v2x * v2x + v2y * v2y);


    double angle = std::acos(dotProduct / (length1 * length2));
    return angle;
}




//==========================================================================================================================================

// 计算质心
CentroidPoint calculateCentroid(const std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>, std::shared_ptr<ReflectorPanelPoint>>>& polygon) {
    double sumX = 0.0, sumY = 0.0, sumZ = 0.0;
    int numPoints = 0;

    for (const auto& edge : polygon) {
        const auto& start = std::get<0>(edge);
        const auto& end = std::get<1>(edge);

        sumX += start->x + end->x;
        sumY += start->y + end->y;
        sumZ += start->z + end->z;
        numPoints += 2; // 每条边提供两个点


    }
    CentroidPoint centroidPoint;
    centroidPoint.x = sumX / numPoints;
    centroidPoint.y = sumY / numPoints;
    centroidPoint.z = sumZ / numPoints;
    return centroidPoint;
}



//======================================================================================================================================
bool findPolygonIndicesNewWithQueueNew1(

        const std::unordered_map
                <std::string ,
                        std::vector<
                                std::tuple<
                                        std::vector<double>,
                                        std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>
                                >
                        >
                >& edgeMap,

        const std::vector<double>& targetEdges,
        std::vector<double>&  targetAngles,
        double threshold,
        std::map<int,std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> >> & map_targetIndices_rusults



) {

    // 记录开始时间
    auto start = std::chrono::high_resolution_clock::now();

    std::vector<std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double>> targetIndices;
    std::map<int,std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double>>  map_targetIndices;
    std::queue<std::vector<std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double>>> pathsQueue;//std::vector<int>类型，集合中共存储2个int数据。
    for (const auto &pair1 : edgeMap) {
        string startPoint = pair1.first;
        const auto &edgesList = pair1.second;//[ID1,ID3],[ID1,ID4],[ID1,ID5],[ID1,ID6],[ID1,ID7] ] ，起点为ID2 [ [ID2,ID3],[ID2,ID4],[ID2,ID5],[ID2,ID6],[ID2,ID7] ]
        for (const auto &edgeslist : edgesList) {
            const auto& edges = std::get<0>(edgeslist);
            const auto& indicesPair = std::get<1>(edgeslist);// std::vector->[起点位id，末尾点位id]， std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>> indicesPair
            for (size_t i = 0; i < edges.size(); ++i) {
                //edges[i]: 原边长, targetEdges[0]:目标边长，threshold：误差值
                if (areEdgesEqualNew(edges[i], targetEdges[0], threshold)) {

                    if(firstTrilateration_with_yawFig_){
                        double x1 =  base_link_to_map_transform_.getOrigin().getX();
                        double y1 =  base_link_to_map_transform_.getOrigin().getY();
                        double x2 =  indicesPair[i][0]->x;
                        double y2 =  indicesPair[i][0]->y;



                        double distance = std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2));
                        if(distance>=calculateEdge_threshold_){
                            ROS_INFO("1.continue 3568 已经成功定位了，后面需要考虑的是在一定距离内作匹配 if(firstTrilateration_with_yawFig_) ,distance %f,calculateEdge_threshold_ :%f",distance,calculateEdge_threshold_);

                            continue;
                        }

                    }

                    std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>, double> indicesPairVector = std::make_tuple(indicesPair[i], edges[i]);
                    pathsQueue.push({indicesPairVector});//并全部压入队列中，队列中会存在n条数据，每条数据中存在满足第一条边长条件的元素
                }else{

                    // ROS_INFO("1.false 对比变长 areEdgesEqualNew %f,targetEdges[0] :%f,threshold :%f,firstId %s, secondId %s",edges[i],targetEdges[0],threshold,indicesPair[i][0]->id.c_str(),indicesPair[i][1]->id.c_str());

                }
            }
        }//for (const auto &outerTuple : outerVec)
    }//   for (const auto &pair1 : unorderedEdgeMapNew)
    std::cout << "----------------------------------------------" <<  std::endl;// 例：ID1
    std::cout <<YELLOW<<  "1.匹配出 第一条边的 的数量: " << pathsQueue.size() <<RESET<< std::endl;// 例：ID1

    std::cout << "----------------------------------------------" <<  std::endl;// 例：ID1


    for (const double& edge : targetEdges) {
        std::cout << "targetEdges 3590 边长: " << std::endl;
        std::cout << edge << "\n";
        if (    std::isnan(edge) || std::isinf(edge)) {
            std::cout << "edge is a valid isnan or finite number.不能加入边长计算。" << std::endl;

        }
    }

//遍历后面的边
    int fritIndexId=0;
    int aaaa=0;
    while (!pathsQueue.empty()) {
        if(!iswileTure_){
            return false;
        }
        aaaa++;
        std::vector<std::vector<std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double>>> all_paths; // 存储所有找到的路径; [sukai 提取出来] 存储所有路径的id
        std::vector<std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double>> currentPathWithPath = pathsQueue.front();//取出第一个数据，//广度优先算法，第一次存一条匹配好的数，而这条数据会放在vector集合中然后压入pathsQueue队列，第二次会在第一次的vector 基础上增加一条边的数据，所以 pathsQueue中的集合的size即表示当前匹配到了第currentPathWithPath.size()条数据。

        pathsQueue.pop();//删除第一个数据

        std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> first_node  =  currentPathWithPath[0];// 取出路径中的第一个节点
        std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> last_node  =  currentPathWithPath.back();// 取出路径中的最后一个节点
        std::vector<std::shared_ptr<ReflectorPanelPoint>>& first_reflectorPanelPoints = std::get<0>(first_node);
        std::vector<std::shared_ptr<ReflectorPanelPoint>>& last_reflectorPanelPoints = std::get<0>(last_node);
        if(first_reflectorPanelPoints.size()<=1||last_reflectorPanelPoints.size()<=1)
            continue;
        std::shared_ptr<ReflectorPanelPoint> first_node_startPoint = first_reflectorPanelPoints[0];
        std::shared_ptr<ReflectorPanelPoint> last_node_endPoint = last_reflectorPanelPoints[1];
        fritIndexId = currentPathWithPath.size();//这个中包含几个元素就等于这个索引去匹配目标边长 targetEdges  [currentPathWithPath 可以理解为存的是一个 vector集合，而这个集合中会把匹配到的数据存进来，具体数量是多少，就代表匹配第多少次，第一次在wile外执行的 ]


        if (currentPathWithPath.size()==(targetEdges.size())) { // 如果最后一个节点是目标节点，则将此路径存储到all_paths中 【 已经找到一条从start到end的路径】

            //all_paths.push_back(currentPathWithPath);
            if (first_node_startPoint->id == last_node_endPoint->id) {
                std::cout << "5.匹配成功 第 " <<currentPathWithPath.size() <<" 条边的 第二个 id Key: " << last_node_endPoint->id<<RESET<< std::endl;// 例：ID1

                std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> > targetIndices_rusult;//找到所有反光板的id,并反回
                for (size_t i = 0; i < currentPathWithPath.size(); ++i) {
                    std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> tuple_node  =  currentPathWithPath[i];// 取出路径中的第一个节点
                    map_targetIndices[i] = tuple_node;
                    std::vector<std::shared_ptr<ReflectorPanelPoint>>& tuple_node_reflectorPanelPoints = std::get<0>(tuple_node);
                    std::shared_ptr<ReflectorPanelPoint> first_node_startPoint2 = tuple_node_reflectorPanelPoints[0];
                    std::shared_ptr<ReflectorPanelPoint> last_node_endPoint2 = tuple_node_reflectorPanelPoints[1];

                    //封装数据
                    std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> indicesPairVector= std::make_tuple(first_node_startPoint2,last_node_endPoint2);
                    targetIndices_rusult.push_back(indicesPairVector);

                }
                // 记录结束时间
                auto end = std::chrono::high_resolution_clock::now();
                // 计算并转换为毫秒
                auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);




                int mapkey=map_targetIndices_rusults.size()+1;
                //todo 匹配成功，反回数据
                map_targetIndices_rusults[mapkey]= targetIndices_rusult;

//                if(!firstTrilateration_with_yawFig_){//第一次
//                    //todo 匹配成功，反回数据
//                    return true;
//                }
                //break;//找到全部数据，跳出循环
            }

            continue;
        }
        else if(currentPathWithPath.size()>(targetEdges.size())){
            std::cout << "6.匹配失败 第 " <<currentPathWithPath.size() <<" 条边的 第一个 id Key: " << first_node_startPoint->id << std::endl;// 例：ID1
            break;
        }else{
            std::cout << MAGENTA<< "====== 正在匹配中 ==================================== if(currentPathWithPath.size()>(targetEdges.size()))=========================================================" << std::endl;
        }

//===============================================================================================


        double  targetEdge = targetEdges[fritIndexId];//fritIndexId=1时，查找第二个元素去匹配。应为第一个元素在wile之前先做好了。
        double  targetAngle = targetAngles[fritIndexId];//fritIndexId=1时，查找第二个元素去匹配。应为第一个元素在wile之前先做好了。


        if(currentPathWithPath.size()<targetEdges.size()&&currentPathWithPath.size()!=(targetEdges.size()-1)) {
            std::vector<
                    std::tuple<
                            std::vector<double>,
                            std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>
                    >
            >  edgeMap_vector =edgeMap.at(last_node_endPoint->id);//todo 拿到最新一条边的末尾id，找到所有以这个点位 id 为起始点的边长数据 edgeMap_vector


            // todo 这段代码是打印数据排除问题使用，并没有实际意义
            for (size_t ii = 0; ii < currentPathWithPath.size(); ++ii) {
                std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> tuple_node_test  =  currentPathWithPath[ii];// 取出路径中的第一个节点
                std::vector<std::shared_ptr<ReflectorPanelPoint>>& tuple_node_reflectorPanelPoints_test = std::get<0>(tuple_node_test);
                std::shared_ptr<ReflectorPanelPoint> first_node_startPoint_test  = tuple_node_reflectorPanelPoints_test[0];
                std::shared_ptr<ReflectorPanelPoint> last_node_endPoint_test = tuple_node_reflectorPanelPoints_test[1];
                std::cout <<GREEN<< "------------------------------------------------------------"  << std::endl;
                std::cout << "当前找到已有数据 第"<< (ii+1) <<" 条直线的起始点 ： " << first_node_startPoint_test->id <<" ,条直线的末尾点 ： " << last_node_endPoint_test->id << std::endl;
                std::cout << "------------------------------------------------------------"  <<RESET<< std::endl;

            }
            //================================================================================================



            bool isContinue = false;
            for (const auto &pair_edgeMap_vector : edgeMap_vector) { //c++ 14
                const auto& edges = std::get<0>(pair_edgeMap_vector);//std::vector [double,double] 边长
                const auto& indicesPair = std::get<1>(pair_edgeMap_vector);//vector[vector [startId1，endId1],[startId2，endId2] ]



                for (int i = 0; i < edges.size(); ++i) {
                    std::shared_ptr<ReflectorPanelPoint> start_node_id = indicesPair[i][0];//startId1 边的起始点
                    std::shared_ptr<ReflectorPanelPoint> end_node_id = indicesPair[i][1]; //endId1    边的末尾点

                    if(currentPathWithPath.size()<(targetEdges.size()-1)){//到最后第二条。indicesPair+1 =》currentPathWithPath.size()：5 =》indicesPair就是第6条。 第7条是末尾id需要与第一条边的id一致的。
                        for (size_t i = 0; i < currentPathWithPath.size(); ++i) {

                            std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> tuple_node  =  currentPathWithPath[i];// 取出路径中的第一个节点
                            std::vector<std::shared_ptr<ReflectorPanelPoint>>& tuple_node_reflectorPanelPoints = std::get<0>(tuple_node);
                            // 一条直线的起始点  std::vector [起点位id，末尾点位id]
                            std::shared_ptr<ReflectorPanelPoint> first_node_startPoint3 = tuple_node_reflectorPanelPoints[0];
                            //一条直线的末尾点 std::vector [起点位id，末尾点位id]
                            std::shared_ptr<ReflectorPanelPoint> last_node_endPoint3 = tuple_node_reflectorPanelPoints[1];

                            if(end_node_id->id == first_node_startPoint3->id || end_node_id->id == last_node_endPoint3->id){

                                isContinue = true;
                                //break;//跳出当前for循环
                                continue;
                            }
                            //========================================================

                        }// for (size_t i = 0; i < currentPathWithPath.size(); ++i)
                    }//if(currentPathWithPath.size()<targetEdges.size()-1)

                    if(isContinue){
                        isContinue = false;
                        continue;
                    }




                    bool areAnglesEqualNewflg = false; //对比角度值
                    bool targetAngleIsnanflg = false; //对比角度值
                    if (areEdgesEqualNew(edges[i], targetEdge, threshold)) {//targetEdges[0]：第一条边长 ，这里主要是找出所有满足条件的第一条边长的所有数据。因为满足单条边长条件的数据不止一条。

                        std::vector<std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double>> newPath =   currentPathWithPath;

                        targetAngle =   targetAngles[newPath.size()-1];//targetAngles 中的第一个夹角，如果当前点是第一个点，则前一个点是最后一个点。（ 第一个夹角：  前一个点-当前点-最后一个点 ）



                        if (    std::isnan(targetAngle) || std::isinf(targetAngle)) {
                            //std::cout << "targetAngle is a valid isnan or finite number. 不能加入角度计算" << std::endl;
                            targetAngleIsnanflg=true;//targetAngleIsnanflg=true : 不能加入角度计算
                        }



                        if(!isStartAreAnglesEqualNew_||targetAngleIsnanflg){//不加入夹角条件
                            // std::cout << "3.不加入夹角条件" << std::endl;


                            //todo 2.已经成功定位了，后面需要考虑的是在一定距离内作匹配
                            if(firstTrilateration_with_yawFig_){

                                double x1 =  base_link_to_map_transform_.getOrigin().getX();
                                double y1 =  base_link_to_map_transform_.getOrigin().getY();
                                double x2 =  indicesPair[i][0]->x;
                                double y2 =  indicesPair[i][0]->y;
                                double distance = std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2));
                                if(distance>=calculateEdge_threshold_){
                                    ROS_INFO("3. continue 3819 已经成功定位了，后面需要考虑的是在一定距离内作匹配,计算距离阀值，两点过远就不需要计算了");
                                    continue;
                                }

                            }

                            //  if (currentPathWithPath.size()!=(targetEdges.size()-1)){
                            std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> indicesPairVector= std::make_tuple(indicesPair[i],edges[i]);
                            newPath.push_back(indicesPairVector);
                            pathsQueue.push(newPath);//todo end 满足条件的数据加入pathsQueue队列


                            std::cout << "3.匹配出 长度: edges[i] " <<std::to_string(edges[i]) <<" targetEdge: " << std::to_string(targetEdge) <<RESET<< std::endl;// 例：ID1


                        }
                            // /**
                        else  if(isStartAreAnglesEqualNew_){ //加入夹角匹配, todo 是否加入两线段之间的夹角作为匹配条件，isStartAreAnglesEqualNew_：true 加入夹角条件

                            //  std::cout << "4.加入角度计算" << std::endl;
                            if(newPath.size()<2){//小于2条边是无法计算角度的
                                //      std::cout << "5.加入角度计算 continue,newPath.size(): " <<newPath.size()<< std::endl;
                                //  if (currentPathWithPath.size()!=(targetEdges.size()-1)){
                                std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> indicesPairVector= std::make_tuple(indicesPair[i],edges[i]);
                                newPath.push_back(indicesPairVector);
                                pathsQueue.push(newPath);//todo end 满足条件的数据加入pathsQueue队列
                                std::cout <<RED<< "4.匹配出 第 " <<newPath.size() <<" 条边的 第一个 id Key: " << indicesPair[i][0]->id << std::endl;// 例：ID1
                                std::cout << "4.匹配出 第 " <<newPath.size() <<" 条边的 第二个 id Key: " << indicesPair[i][1]->id <<RESET<< std::endl;// 例：ID1
                                continue;
                            }
                            // std::cout << "6.加入角度计算" << std::endl;
                            //   if(isStartAreAnglesEqualNew_&&newPath.size()>1){ //夹角匹配
                            //前一条直线的2个点
                            std::vector<std::shared_ptr<ReflectorPanelPoint>> reflectorPanelPoint_vector_first =  std::get<0>(newPath[newPath.size()-2]);

                            //后一条直线的2个点
                            std::vector<std::shared_ptr<ReflectorPanelPoint>> reflectorPanelPoint_vector_end =  std::get<0>(newPath[newPath.size()-1]);
                            if(reflectorPanelPoint_vector_first.size()==0||reflectorPanelPoint_vector_end.size()==0){
                                //  if (currentPathWithPath.size()!=(targetEdges.size()-1)){
                                std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> indicesPairVector= std::make_tuple(indicesPair[i],edges[i]);
                                newPath.push_back(indicesPairVector);
                                pathsQueue.push(newPath);//todo end 满足条件的数据加入pathsQueue队列
                                //  std::cout << "7.加入角度计算 continue" << std::endl;
                                std::cout << "5.匹配出 第 " <<newPath.size() <<" 条边的 第二个 id Key: " << indicesPair[i][1]->id << RESET<<std::endl;// 例：ID1
                                continue;
                            }
                            // std::cout << "8.加入角度计算" << std::endl;
                            //前一条直线的2个点
                            std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint_first1   =     reflectorPanelPoint_vector_first.front();//起点

                            std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint_first2   =     reflectorPanelPoint_vector_first.back();//终点


                            //后一条直线的2个点
                            std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint_end1   =     reflectorPanelPoint_vector_end.front();//起点

                            std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint_end2   =     reflectorPanelPoint_vector_end.back();//终点

                            Point p1;
                            p1.x = reflectorPanelPoint_first1->x;
                            p1.y = reflectorPanelPoint_first1->y;
                            Point p2;
                            p2.x = reflectorPanelPoint_first2->x;
                            p2.y = reflectorPanelPoint_first2->y;
                            Point p3;
                            p3.x= reflectorPanelPoint_end2->x;
                            p3.y = reflectorPanelPoint_end2->y;

                            double sourceAngle =   calculateAngle(p1,  p2, p3);

                            //todo 2. 对比夹角
                            areAnglesEqualNewflg =   areAnglesEqualNew(sourceAngle, targetAngle, anglesthreshold_);
                            //  std::cout << "9.加入角度计算" << std::endl;
                            if(areAnglesEqualNewflg){//加入夹角条件

                                if(firstTrilateration_with_yawFig_){
                                    double x1 =  base_link_to_map_transform_.getOrigin().getX();
                                    double y1 =  base_link_to_map_transform_.getOrigin().getY();
                                    double x2 =  indicesPair[i][0]->x;
                                    double y2 =  indicesPair[i][0]->y;
                                    double distance = std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2));
                                    if(distance>=calculateEdge_threshold_){
                                        //计算距离阀值，两点过远就不需要计算了
                                        ROS_INFO("6.continue 3914 计算距离阀值，两点过远就不需要计算了,distance %f , %f",distance,calculateEdge_threshold_);
                                        std::cout <<MAGENTA<< "6. continue==================newPath.size() " <<newPath.size() <<" 条边"<<  std::endl;// 例：ID
                                        std::cout <<MAGENTA<< "6. continue ==================pathsQueue.size() " <<pathsQueue.size() <<" 条边"<<  std::endl;// 例：ID
                                        //  std::cout << "11.加入角度计算 continue" << std::endl;
                                        continue;
                                    }

                                }

                                std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> indicesPairVector= std::make_tuple(indicesPair[i],edges[i]);
                                newPath.push_back(indicesPairVector);
                                pathsQueue.push(newPath);//todo end 满足条件的数据加入pathsQueue队列
                                // std::cout << "12.加入角度计算 " << std::endl;
                                std::cout << MAGENTA<<"6.匹配出 第 " <<newPath.size() <<" 条边的 第一个 id Key: " << indicesPair[i][0]->id << std::endl;// 例：ID1
                                std::cout << "----------------------------------------------" <<  std::endl;// 例：ID1
                                std::cout << "6.匹配出 第 " <<newPath.size() <<" 条边的 第二个 id Key: " << indicesPair[i][1]->id <<MAGENTA<< std::endl;// 例：ID1
                                std::cout <<MAGENTA<< "6.==================newPath.size() " <<newPath.size() <<" 条边"<<  std::endl;// 例：ID
                                std::cout <<MAGENTA<< "6.==================pathsQueue.size() " <<pathsQueue.size() <<" 条边"<<  std::endl;// 例：ID

                            }else{
                                //  ROS_INFO("3.false 对比夹角 areAnglesEqualNew %f,targetAngle :%f,anglesthreshold_ :%f,p1 reflectorPanelPoint_first1Id %s,p2 reflectorPanelPoint_first2Id %s,p3 reflectorPanelPoint_end2 %s",sourceAngle,targetAngle,anglesthreshold_,reflectorPanelPoint_first1->id.c_str(),reflectorPanelPoint_first2->id.c_str(),reflectorPanelPoint_end2->id.c_str());

                            }


                        }



                    }//if (areEdgesEqualNew(edges[i], targetEdge, threshold)) {
                    else{
                        //ROS_INFO("2.false 对比变长 areEdgesEqualNew %f,targetEdge :%f,threshold :%f,firstId %s, secondId %s",edges[i],targetEdge,threshold,indicesPair[i][0]->id.c_str(),indicesPair[i][1]->id.c_str());

                    }


                }

            }// for (const auto &[edges, indicesPair]: edgeMap_vector)


        }//if (currentPathWithPath.size()==targetEdges.size()&&first_node_startPoint->id == last_node_endPoint->id)
        else if(currentPathWithPath.size()==(targetEdges.size()-1)){
            std::vector<
                    std::tuple<
                            std::vector<double>,
                            std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>
                    >
            >  edgeMap_vector =edgeMap.at(last_node_endPoint->id);//todo 拿到最新一条边的末尾id，找到所有以这个点位 id 为起始点的边长数据 edgeMap_vector

//   for (const auto &[edges, indicesPair]: edgeMap_vector) { //c++ 17
            bool isContinue = false;
            for (const auto &pair_edgeMap_vector : edgeMap_vector) { //c++ 14
                const auto& edges = std::get<0>(pair_edgeMap_vector);//std::vector [double,double] 边长
                const auto& indicesPair = std::get<1>(pair_edgeMap_vector);//vector[vector [startId1，endId1],[startId2，endId2] ]

//edges  std::vector [double,double]
                for (int i = 0; i < indicesPair.size(); ++i) {
                    std::shared_ptr<ReflectorPanelPoint> start_node_id = indicesPair[i][0];//startId1 边的起始点
                    std::shared_ptr<ReflectorPanelPoint> end_node_id = indicesPair[i][1]; //endId1    边的末尾点
                    //找到最后一条边尾id 与第一条边的首id一致的数据
                    if(end_node_id->id==first_node_startPoint->id){
                        //判断变长是否符合条件
                        bool areAnglesEqualNewflg = false; //对比角度值
                        bool targetAngleIsnanflg = false; //对比角度值
                        double threshold_1 = threshold*1.5;
                        if (areEdgesEqualNew(edges[i], targetEdge, threshold_1)) {//targetEdges[0]：第一条边长 ，这里主要是找出所有满足条件的第一条边长的所有数据。因为满足单条边长条件的数据不止一条。

                            std::vector<std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double>> newPath =   currentPathWithPath;

                            targetAngle =   targetAngles[newPath.size()-1];//targetAngles 中的第一个夹角，如果当前点是第一个点，则前一个点是最后一个点。（ 第一个夹角：  前一个点-当前点-最后一个点 ）



                            if (    std::isnan(targetAngle) || std::isinf(targetAngle)) {
                                //std::cout << "targetAngle is a valid isnan or finite number. 不能加入角度计算" << std::endl;
                                targetAngleIsnanflg=true;//targetAngleIsnanflg=true : 不能加入角度计算
                            }



                            if(!isStartAreAnglesEqualNew_||targetAngleIsnanflg){//不加入夹角条件
                                // std::cout << "3.不加入夹角条件" << std::endl;


                                //todo 2.已经成功定位了，后面需要考虑的是在一定距离内作匹配
                                if(firstTrilateration_with_yawFig_){

                                    double x1 =  base_link_to_map_transform_.getOrigin().getX();
                                    double y1 =  base_link_to_map_transform_.getOrigin().getY();
                                    double x2 =  indicesPair[i][0]->x;
                                    double y2 =  indicesPair[i][0]->y;
                                    double distance = std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2));
                                    if(distance>=calculateEdge_threshold_){
                                        ROS_INFO("3. continue 3819 已经成功定位了，后面需要考虑的是在一定距离内作匹配,计算距离阀值，两点过远就不需要计算了");
                                        continue;
                                    }

                                }

                                //  if (currentPathWithPath.size()!=(targetEdges.size()-1)){
                                std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> indicesPairVector= std::make_tuple(indicesPair[i],edges[i]);
                                newPath.push_back(indicesPairVector);
                                pathsQueue.push(newPath);



                            }
                                // /**
                            else  if(isStartAreAnglesEqualNew_){

                                if(newPath.size()<2){
                                    std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> indicesPairVector= std::make_tuple(indicesPair[i],edges[i]);
                                    newPath.push_back(indicesPairVector);
                                    pathsQueue.push(newPath);
                                    std::cout << "4.匹配出 第 " <<newPath.size() <<" 条边的 第二个 id Key: " << indicesPair[i][1]->id <<RESET<< std::endl;// 例：ID1
                                    continue;
                                }
                                // std::cout << "6.加入角度计算" << std::endl;
                                //   if(isStartAreAnglesEqualNew_&&newPath.size()>1){ //夹角匹配
                                //前一条直线的2个点
                                std::vector<std::shared_ptr<ReflectorPanelPoint>> reflectorPanelPoint_vector_first =  std::get<0>(newPath[newPath.size()-2]);

                                //后一条直线的2个点
                                std::vector<std::shared_ptr<ReflectorPanelPoint>> reflectorPanelPoint_vector_end =  std::get<0>(newPath[newPath.size()-1]);
                                if(reflectorPanelPoint_vector_first.size()==0||reflectorPanelPoint_vector_end.size()==0){
                                    //  if (currentPathWithPath.size()!=(targetEdges.size()-1)){
                                    std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> indicesPairVector= std::make_tuple(indicesPair[i],edges[i]);
                                    newPath.push_back(indicesPairVector);
                                    pathsQueue.push(newPath);

                                    std::cout << "5.匹配出 第 " <<newPath.size() <<" 条边的 第二个 id Key: " << indicesPair[i][1]->id << RESET<<std::endl;// 例：ID1
                                    continue;
                                }

                                std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint_first1   =     reflectorPanelPoint_vector_first.front();//起点

                                std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint_first2   =     reflectorPanelPoint_vector_first.back();//终点

                                std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint_end1   =     reflectorPanelPoint_vector_end.front();//起点

                                std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint_end2   =     reflectorPanelPoint_vector_end.back();//终点

                                Point p1;
                                p1.x = reflectorPanelPoint_first1->x;
                                p1.y = reflectorPanelPoint_first1->y;
                                Point p2;
                                p2.x = reflectorPanelPoint_first2->x;
                                p2.y = reflectorPanelPoint_first2->y;
                                Point p3;
                                p3.x= reflectorPanelPoint_end2->x;
                                p3.y = reflectorPanelPoint_end2->y;

                                double sourceAngle =   calculateAngle(p1,  p2, p3);


                                areAnglesEqualNewflg =   areAnglesEqualNew(sourceAngle, targetAngle, anglesthreshold_);

                                if(areAnglesEqualNewflg){//加入夹角条件

                                    //todo 3.已经成功定位了，后面需要考虑的是在一定距离内作匹配
                                    if(firstTrilateration_with_yawFig_){
                                        double x1 =  base_link_to_map_transform_.getOrigin().getX();
                                        double y1 =  base_link_to_map_transform_.getOrigin().getY();
                                        double x2 =  indicesPair[i][0]->x;
                                        double y2 =  indicesPair[i][0]->y;
                                        double distance = std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2));
                                        if(distance>=calculateEdge_threshold_){
                                            //计算距离阀值，两点过远就不需要计算了
                                            ROS_INFO("6.continue 3914 计算距离阀值，两点过远就不需要计算了,distance %f , %f",distance,calculateEdge_threshold_);
                                            std::cout <<MAGENTA<< "6. continue==================newPath.size() " <<newPath.size() <<" 条边"<<  std::endl;// 例：ID
                                            std::cout <<MAGENTA<< "6. continue ==================pathsQueue.size() " <<pathsQueue.size() <<" 条边"<<  std::endl;// 例：ID
                                            //  std::cout << "11.加入角度计算 continue" << std::endl;
                                            continue;
                                        }

                                    }

                                    std::tuple<std::vector<std::shared_ptr<ReflectorPanelPoint>>,double> indicesPairVector= std::make_tuple(indicesPair[i],edges[i]);
                                    newPath.push_back(indicesPairVector);
                                    pathsQueue.push(newPath);

                                    std::cout <<MAGENTA<< "6.==================pathsQueue.size() " <<pathsQueue.size() <<" 条边"<<  std::endl;// 例：ID

                                }else{
                                    //  ROS_INFO("3.false 对比夹角 areAnglesEqualNew %f,targetAngle :%f,anglesthreshold_ :%f,p1 reflectorPanelPoint_first1Id %s,p2 reflectorPanelPoint_first2Id %s,p3 reflectorPanelPoint_end2 %s",sourceAngle,targetAngle,anglesthreshold_,reflectorPanelPoint_first1->id.c_str(),reflectorPanelPoint_first2->id.c_str(),reflectorPanelPoint_end2->id.c_str());
                                    continue;
                                }


                            }



                        }//if (areEdgesEqualNew(edges[i], targetEdge, threshold)) {
                        else{
                            ROS_INFO("3.2.error 最后一条边不满足边长要求 false if (areEdgesEqualNew(edges[i], targetEdge, threshold_1))  对比变长 areEdgesEqualNew %f,targetEdge :%f,threshold :%f,firstId %s, secondId %s",edges[i],targetEdge,threshold_1,indicesPair[i][0]->id.c_str(),indicesPair[i][1]->id.c_str());
                            continue;
                        }



                    }// if(end_node_id->id==first_node_startPoint->id){
                    else{
                        continue;
                    }

                }//for (int i = 0; i < indicesPair.size(); ++i) {




            }// for (const auto &[edges, indicesPair]: edgeMap_vector)


        }else{

            std::cout <<RED<< "8.==================pathsQueue.size() " <<pathsQueue.size() <<" 条边"<<  std::endl;// 例：ID
            //return false;
            continue;

        }



    } //while (!pathsQueue.empty())


    if(map_targetIndices_rusults.size()==0){
        std::cout <<RED<< "2. 3964 没有找到多边形特征数据 map_targetIndices_rusults.size() "<<map_targetIndices_rusults.size()<<RESET<<std::endl;
        return false;
    }

// 记录结束时间
    auto end = std::chrono::high_resolution_clock::now();
// 计算并转换为毫秒
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);

// 输出执行时间
    std::cout << "匹配执行时间 Function took " << duration.count() << " milliseconds to execute.\n";


// 如果没有找到符合条件的闭环路径，则返回空向量
    // return targetIndices_min_rusult;
    return true;
}






// 封装的函数
bool findAndPrintData(const std::unordered_map<std::vector<std::string>, std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>>, VectorStringHash, VectorStringEqual>& map, const std::vector<std::string>& key , std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>> &reslutData,double &result_distance) {
    auto it = map.find(key);
    std::cout << "std::unordered_map: " << std::endl;
    std::cout << "key-> std::vector<std::string> "  <<std::endl;
    std::cout << "std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>>"  <<std::endl;
    std::cout << "--------"<< std::endl;

    if (it != map.end()) {
        reslutData=it->second;
        std::cout << "Found key: ";
        for (const auto& str : it->first) {
            std::cout << str << " ";
        }
        std::cout << std::endl;
        // 打印值
        const auto& innerVec1 = std::get<0>(it->second);
        const auto& innerVec2 = std::get<1>(it->second);
        result_distance=innerVec1;
        std::cout << "边长    Tuple - double : ";
        std::cout << innerVec1 << " ";
        std::cout << std::endl;

        std::cout << "对应边起终点id   Tuple - Vector : "<< std::endl;
        for (const auto& innerVec : innerVec2) {
            for (const auto& ptr : innerVec) {
                std::cout <<"                  "<< "[ ";
                // 使用 ptr 指针
                std::cout  <<  ptr->id<<" ： ";
                std::cout << " ( " << ptr->x <<" , "<<   ptr->y <<" ) ";
                std::cout << "] "<< std::endl;
            }
        }
        std::cout << std::endl;

        return true;
    } else {
        std::cout << "Key not found." << std::endl;
    }
    return false;
}

// 打印 rviz点击获取到的点位 pointsSaveMap_的内容
void printPointsSaveMap() {
    std::cout << "pointsSaveMap_:\n";
    for (const auto& pair : pointsSaveMap_) {
        std::cout << "Key: " << pair.first << ", x: " << pair.second.x
                  << ", y: " << pair.second.y <<
                  ", z: " << pair.second.z <<
                  ", range: " << pair.second.range <<
                  ", intensity: " << pair.second.intensity <<
                  ", shape: " << pair.second.shape <<", id: " << pair.second.id <<"\n";
    }
}
// 打印 所有反光板  pointAllMaps_的内容
void printPointAllMaps() {

    for (const auto &entry : reflectorPanelPointAllMaps_) {
        // entry.first 是键（key），即std::string类型，代表反射板的标识符或名称
        const std::string &panelName = entry.first;
        // entry.second 是值（value），即std::shared_ptr<ReflectorPanelPoint>类型，指向反射板点的智能指针
        const std::shared_ptr<ReflectorPanelPoint> &panelPoint = entry.second;

        // 现在可以访问或操作panelPoint指向的对象了
        // 例如，打印反射板点的一些信息
        std::cout << "Panel Name: " << panelName << ", Point Info: ";
        if (panelPoint) { // 检查智能指针是否有效
            std::cout << panelPoint; // 假设ReflectorPanelPoint类有一个printInfo()成员函数
        } else {
            std::cout << "nullptr";
        }
        std::cout << std::endl;
    }



}

// 打印 所有反光板 排序好的数据据
void printSortSetPeriodically() {
    iswirtortSetPeriodically_=true;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    for (const auto &entry : reflectorPanelPoints_vector_) {
        // entry.second 是值（value），即std::shared_ptr<ReflectorPanelPoint>类型，指向反射板点的智能指针
        const std::shared_ptr<ReflectorPanelPoint> &panelPoint = entry;

        // 现在可以访问或操作panelPoint指向的对象了
        // 例如，打印反射板点的一些信息
        std::cout << "Panel Name: " << panelPoint->id << ", Point Info: ";
        if (panelPoint) { // 检查智能指针是否有效
            std::cout << panelPoint; // 假设ReflectorPanelPoint类有一个printInfo()成员函数
        } else {
            std::cout << "nullptr";
        }
        std::cout << std::endl;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    iswirtortSetPeriodically_=false;

}
// 封装的计算好的函数
void printUnorderedEdgeMapNew(const  std::unordered_map<std::string , std::vector<std::tuple<std::vector<double>, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>>>& unorderedEdgeMapNew) {
    std::cout << "printUnorderedEdgeMapNew";
    std::cout << "std::unordered_map: " << std::endl;
    std::cout << "key-> std::string ,value->std::vector<std::tuple<std::vector<double>"  <<std::endl;
    std::cout << "std::vector<ststd::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>  d::vector<std::shared_ptr<ReflectorPanelPoint>>>"  <<std::endl;
    std::cout << "--------"<< std::endl;
    int i=1;
    // 遍历 unorderedEdgeMapNew
    for (const auto& pair : unorderedEdgeMapNew) {

        std::cout << "num: "<<i<< std::endl;
        std::cout << "Key: " << pair.first << std::endl;
        const auto& outerVec = pair.second;
        for (const auto& outerTuple : outerVec) {
            const auto& edges = std::get<0>(outerTuple);
            const auto& innerVec2 = std::get<1>(outerTuple);

            std::cout << "边长 Tuple - Vector : ";
            for (const auto& edge : edges) {
                std::cout << edge << " ";
            }
            std::cout << std::endl;

            std::cout << "对应边起终点id   Tuple - Vector : "<< std::endl;
            for (const auto& innerVec : innerVec2) {
                for (const auto& ptr : innerVec) {
                    std::cout <<"                  "<< "[ ";
                    // 使用 ptr 指针
                    std::cout  <<  ptr->id<<" ： ";
                    std::cout << " ( " << ptr->x <<" , "<<   ptr->y <<" ) ";
                    std::cout << "] "<< std::endl;
                }
            }


            std::cout << "--------------------------------------------------"<< std::endl;
            std::cout << std::endl;
            i++;

        }//for (const auto& outerTuple : outerVec) {
    }
    std::cout << "unorderedEdgeMapNew.size()：" << unorderedEdgeMapNew.size()<<std::endl;
    std::cout << "---------------------------------------------------------------------------" <<std::endl;
    int count=0;
    for (const auto& pair1 : unorderedEdgeMapNew) {
        const std::string& id1 = pair1.first;
        std::vector<std::tuple<std::vector<double>, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>> point = pair1.second;
        // std::cout << "point.size()：" << point.size()<<std::endl;
        count+=point.size();
    }
    std::cout << "总共 ： " << count<<" 种组合。"<<std::endl;


}




// 封装的计算好函数
void printUnorderedEdgeMapNew_IdKey(const std::unordered_map<std::vector<std::string>, std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>>, VectorStringHash, VectorStringEqual>& unorderedEdgeMapNew_IdKey) {
    std::cout << "printUnorderedEdgeMapNew_IdKey";
    std::cout << "std::unordered_map: " << std::endl;
    std::cout << "key-> std::vector<std::string> "  <<std::endl;
    std::cout << "value->std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>>"  <<std::endl;
    std::cout << "--------"<< std::endl;
    int i=1;
    for (const auto& pair : unorderedEdgeMapNew_IdKey) {

        std::cout << "num: "<<i<< std::endl;
        std::cout << "Key: ";
        for (const auto& str : pair.first) {
            std::cout << str << " ";
        }
        std::cout << std::endl;

        // 打印值
        const auto& innerVec1 = std::get<0>(pair.second);
        const auto& innerVec2 = std::get<1>(pair.second);
        std::cout << "边长   Tuple : ";
        std::cout << innerVec1 << " ";
        std::cout << std::endl;
        std::cout << "对应边起终点id   Tuple - Vector : "<< std::endl;
        for (const auto& innerVec : innerVec2) {
            for (const auto& ptr : innerVec) {
                std::cout <<"                  "<< "[ ";
                // 使用 ptr 指针
                std::cout  <<  ptr->id<<" ： ";
                std::cout << " ( " << ptr->x <<" , "<<   ptr->y <<" ) ";
                std::cout << "] "<< std::endl;
            }
        }
        //    std::cout << std::endl;


        std::cout << "--------------------------------------------------"<< std::endl;
        std::cout << std::endl;
        i++;
    }
    std::cout << "unorderedEdgeMapNew_IdKey.size()：" << unorderedEdgeMapNew_IdKey.size()<<std::endl;
}

// 打印识别完成的反光柱
void printIndices(const std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> > & indices) {
    std::cout << "--------"<< std::endl;
    // 打印结果
    if (!indices.empty()) {

        std::cout <<GREEN<< "printIndices";
        std::cout << "std::vector: " << std::endl;
        std::cout << "std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>>>"  <<std::endl;
        //std::cout << "--------"<< std::endl;
        // 遍历 unorderedEdgeMapNew
        std::cout << "找到匹配的多边形 " << std::endl;
        std::cout << "反光柱数量： " <<indices.size()<< std::endl;
        for (std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>>   reflectorPanelPoint : indices) {

            std::shared_ptr<ReflectorPanelPoint> & point_frist = std::get<0>(reflectorPanelPoint);//一条直线的起始点
            std::shared_ptr<ReflectorPanelPoint> & point_end  = std::get<1>(reflectorPanelPoint);//一条直线的终点

            std::cout << point_frist->id << " ,";//所有直线的起始点
        }
        std::cout << std::endl;
    } else {
        std::cerr <<RED<< "没有找到匹配的多边形" << RESET <<std::endl;
    }


}
//-----------------------封装json数据------------------------------
//----------------------雷达对应的强度数据-------------------------------
//封装雷达对应的强度数的json数据
std::string generateJSONData_with_ScanIntensities( const  std::map<int,double> &scan_intensitie_map) {

/**
    {
           "scan_intensities": [[1,1600.0],[2,1590.0],[5,1580.0],[6,1570.0],[8,1500.0],[1,1400.0],[10,1300]]
    }

*/

    Json json_data;
    for (const auto& entry : scan_intensitie_map) {
        Json item;
        item.append(entry.first);
        item.append(entry.second);
        json_data["scan_intensities"].append(item);
    }
    std::string   jsonData=json_data.str();
    std::cout << "封装雷达对应的强度数的json数据" <<"end"<< std::endl;
    json_data.clear();//用完,内存释放
    return jsonData;
}
//写入txt数据
void saveJSONDataToFile_with_ScanIntensities(const std::string& jsonData, const std::string& filename) {
    std::ofstream outputFile(filename, std::ios::app);
    if (outputFile.is_open()) {
        outputFile << jsonData << std::endl;
        std::cout << "JSON data saved to file: " << filename << std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(50));

    } else {
        std::cerr << "Error: Unable to open file: " << filename << std::endl;
    }
    outputFile.close();

}
//解析 每个反光板数据  scan_intensitie_map_ scan_intensitie_key_
void parseJSONDataWithScanIntensities(const std::string& jsonData,std::map<int,double> &scan_intensitie_map) {

/**
    {
         "scan_intensities": [[1,1600.0],[2,1590.0],[5,1580.0],[6,1570.0],[8,1500.0],[1,1400.0],[10,1300]]
    }
*/

    // 创建解析器对象
    Parser parser;

    // 加载 JSON 字符串
    parser.load(jsonData);
    // 解析 JSON 字符串
    Json json_data = parser.parse();
    std::cout <<BLUE<< "------解析 每个反光板数据------------------------------parseJSONDataWithScanIntensities----------------------------------------------------"  << std::endl;
    // 打印解析后的 JSON 对象
    std::cout << "解析数据 Parsed JSON data:\n" << json_data << std::endl;
    if (!json_data.isNull() &&json_data.isObject() ) {
        Json& scan_intensities = json_data["scan_intensities"];
        if ( !scan_intensities.isNull()  && scan_intensities.isArray()) {
            for ( Json& item : scan_intensities) {
                if (!item.isNull()  && item.isArray() && item.size() == 2) {
                    int key = item[0].isInt() ? item[0].asInt() : 0;
                    double value = item[1].isDouble() ? item[1].asDouble() : item[1].isInt()?item[1].asInt(): 0;
                    scan_intensitie_map[key] = value;
                    scan_intensitie_key_.push_back(key);
                    std::cout << "Key: " << key << ", Value: " << value << std::endl;
                }
            }
        }
    }

    //  json_data.clear();//释放内存
    std::cout <<"解析json得到 每个反光板标定的强度数据 储存于 parseJSONDataWithScanIntensities 集合，当前数据数量： "<<scan_intensitie_map.size()  <<  std::endl;
    std::cout << "----------------------------------------------------------------------------------------"  << RESET <<std::endl;
}

//读取json文件 加载文件中的json 雷达对应的强度数据，存入std::map<int,double> &scan_intensitie_map
void loadScanIntensitiesJSONDataFromFile( std::string& filename,std::map<int,double> &scan_intensitie_map ) {


    std::cout << "-----------------------------------------------读取雷达对应的强度数据-----------------------------------------"  << RESET <<std::endl;

    std::ifstream inputFile(filename);//读取数据
    try{
        if (inputFile.is_open()) {
            std::string line;
            while (std::getline(inputFile, line)) {
                std::istringstream iss(line);
                std::cout<<"loadScanIntensitiesJSONDataFromFile: "<<std::endl;
                std::cout<<line<<std::endl;
                if(line.empty())
                    continue;
                //解析数据 到 scan_intensitie_map
                parseJSONDataWithScanIntensities(line, scan_intensitie_map);

            }
            inputFile.close();
        } else {
            std::cerr << "Error: Unable to open file: " << filename << std::endl;
        }

    }catch(std::exception &e){

        if (inputFile.is_open())
            inputFile.close();

        std::cerr<<"loadAllPointJSONDataFromFile: "<<e.what()<<std::endl;
    }

}



//----------------------单个反光板点位-------------------------------
//解析 每个反光板数据
void parseJSONDataWithAllPoint(const std::string& jsonData,std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> &reflectorPanelPoints_vector, std::map<std::string ,std::shared_ptr<ReflectorPanelPoint>> &reflectorPanelPointAllMaps, std::unordered_map<std::tuple<double, double>, std::shared_ptr<ReflectorPanelPoint>, TupleHash, TupleEqual>  & reflectorPanelPointAllUnordered_maps,int &markerId) {

/**
    {
        "id": "xxxxx-xxxxx",
                "range": 360.0,
                "intensity": 0.05,
                "shape": "CYLINDER",//CUBE：立方体（平面）,CYLINDER: 圆柱体
                "datas": [1.1,1.2,0.0,0.0,0.0,0.0,1.0]
    }
*/

    // 创建解析器对象
    Parser parser;

    // 加载 JSON 字符串
    parser.load(jsonData);
    // 解析 JSON 字符串
    Json json_data = parser.parse();
    std::cout <<BLUE<< "------------------------------------parseJSONDataWithAllPoint----------------------------------------------------"  << std::endl;
    // 打印解析后的 JSON 对象
    std::cout << "解析数据 Parsed JSON data:\n" << json_data << std::endl;
    std::string id=json_data["id"].isNull()?"EMPTY": json_data["id"].isString()?json_data["id"].asString():"";//唯一标示

    std::shared_ptr<ReflectorPanelPoint>  newPoint = ReflectorPanelPoint::createInstance1(id);
    // newPoint->id =json_data["id"].isNull()?"EMPTY": json_data["id"].isString()?json_data["id"].asString():"";//唯一标示
    newPoint->range = json_data["range"].isDouble()?json_data["range"].asDouble():json_data["range"].isInt()?json_data["range"].asInt():0;
    newPoint->intensity = json_data["intensity"].isDouble()?json_data["intensity"].asDouble():json_data["intensity"].isInt()?json_data["intensity"].asInt():0;
    newPoint->shape =json_data["shape"].isNull()?"EMPTY": json_data["shape"].isString()?json_data["shape"].asString():"";
    newPoint->source_frame_id =json_data["source_frame_id"].isNull()?"EMPTY": json_data["source_frame_id"].isString()?json_data["source_frame_id"].asString():"EMPTY";
    newPoint->target_frame_id =json_data["target_frame_id"].isNull()?"EMPTY": json_data["target_frame_id"].isString()?json_data["target_frame_id"].asString():"EMPTY";
    newPoint->x = json_data["datas"][0].isDouble()?json_data["datas"][0].asDouble():json_data["datas"][0].isInt()?json_data["datas"][0].asInt():0;
    newPoint->y = json_data["datas"][1].isDouble()?json_data["datas"][1].asDouble():json_data["datas"][1].isInt()?json_data["datas"][1].asInt():0;
    newPoint->z = json_data["datas"][2].isDouble()?json_data["datas"][2].asDouble():json_data["datas"][2].isInt()?json_data["datas"][2].asInt():0;
    newPoint->rx = json_data["datas"][3].isDouble()?json_data["datas"][3].asDouble():json_data["datas"][3].isInt()?json_data["datas"][3].asInt():0;
    newPoint->ry = json_data["datas"][4].isDouble()?json_data["datas"][4].asDouble():json_data["datas"][4].isInt()?json_data["datas"][4].asInt():0;
    newPoint->rz = json_data["datas"][5].isDouble()?json_data["datas"][5].asDouble():json_data["datas"][5].isInt()?json_data["datas"][5].asInt():0;
    newPoint->rw = json_data["datas"][6].isDouble()?json_data["datas"][6].asDouble():json_data["datas"][6].isInt()?json_data["datas"][6].asInt():0;
    reflectorPanelPointAllMaps[newPoint->id]=newPoint;//map列表储存所有点位，通过tf订阅数据，实现自动排序功能
    newPoint->reflectorPanelPoint_marker_id_=markerId;
    newPoint->reflectorPanelPoint_text_id_=markerId+1;
    reflectorPanelPoints_vector.insert(newPoint);//列表储存所有点位，通过tf订阅数据，实现自动排序功能
    reflectorPanelPointAllUnordered_maps[std::make_tuple(newPoint->x, newPoint->y)]=newPoint;//map列表储存所有点位，(newPoint->x, newPoint->y) 为 key
    markerId=markerId+2;
    //json_data.clear();//释放内存

    //给 pointsSaveMap_中增加数据，方便后续继续在环境中增加反光板数据
    Point point;
    point.id = newPoint->reflectorPanelPoint_marker_id_;
    point.shape=newPoint->shape;
    point.range=scan_size_;
    point.intensity=intensities_size_;

    point.x = newPoint->x;
    point.y = newPoint->y;
    point.z = newPoint->z;
    //当point.rw被设置为1，而point.rx, point.ry, point.rz都是0时，这个四元数表示的是一个没有旋转的状态，即刚体保持其初始方向不变。
    point.rx = newPoint->rx;
    point.ry = newPoint->ry;
    point.rz = newPoint->rz;
    point.rw=  newPoint->rw;
    pointsSaveMap_[point.id]=point;


    //直接打印数据
    std::cout<<newPoint<<std::endl;
    std::cout <<"解析json得到 每个反光板数据 储存于 reflectorPanelPointAllMaps集合，当前数据数量： "<<reflectorPanelPointAllMaps.size()  <<  std::endl;
    std::cout << "----------------------------------------------------------------------------------------"  << RESET <<std::endl;
}

//写入txt数据
void saveJSONDataToFile(std::ofstream &outputFile,const std::string& jsonData, const std::string& filename) {
    std::lock_guard<std::mutex> lock(fileMutex);
    std::lock_guard<std::mutex> lock1(filePointMutex);

    if (outputFile.is_open()) {
        outputFile << jsonData << std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(10));

        std::cout << "JSON data saved to file: " << filename << std::endl;

    } else {
        std::cerr << "Error: Unable to open file: " << filename << std::endl;
    }

}
//封装单个反光板点位的json数据
std::string generateJSONData_with_point(   std::shared_ptr<ReflectorPanelPoint> &reflectorPanelPoint) {

/**
    {
           "id": "xxxxx-xxxxx",
           "range": 360.0,
           "intensity": 0.05,
           "shape": "CYLINDER",//CUBE：立方体（平面）,CYLINDER: 圆柱体
           "datas": [1.1,1.2,0.0,0.0,0.0,0.0,1.0]
    }

*/
    Json json_data;
    json_data["id"]=reflectorPanelPoint->id;
    json_data["range"]=reflectorPanelPoint->range;
    json_data["intensity"]=reflectorPanelPoint->intensity;
    json_data["shape"]=reflectorPanelPoint->shape;
    json_data["source_frame_id"]=reflectorPanelPoint->source_frame_id;
    json_data["target_frame_id"]=reflectorPanelPoint->target_frame_id;
    json_data["datas"].append(reflectorPanelPoint->x);
    json_data["datas"].append(reflectorPanelPoint->y);
    json_data["datas"].append(reflectorPanelPoint->z);
    json_data["datas"].append(reflectorPanelPoint->rx);
    json_data["datas"].append(reflectorPanelPoint->ry);
    json_data["datas"].append(reflectorPanelPoint->rz);
    json_data["datas"].append(reflectorPanelPoint->rw);

    std::string   jsonData=json_data.str();
    json_data.clear();//用完,内存释放
    return jsonData;
}


//  // std::vector 增加数据 储存所有 排序好的反光板数据 自动排序
void addInsertNewPointToReflectorPanelPoints_vector(std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> & reflectorPanelPoints_vector,const std::shared_ptr<ReflectorPanelPoint> & newPoint) {
    iswirtortSetPeriodically_=true;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    std::lock_guard<std::mutex> lock(sortSetPeriodically_mutex_);
    //增加数据
    reflectorPanelPoints_vector.insert(newPoint);
    // 唤醒等待的排序线程
    sortSetPeriodically_cond_var_.notify_one();
    std::cout<<MAGENTA<<"=========std::vector 增加数据  排序好的反光板数据 自动排序======reflectorPanelPoints_vector.size(): " <<reflectorPanelPoints_vector.size()<<RESET<<std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    iswirtortSetPeriodically_=false;

}
//删除数据 自动排序
void deleteNewPointToReflectorPanelPoints_vector(std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> & reflectorPanelPoints_vector,const std::string & reflectorPanelPointId) {
    std::lock_guard<std::mutex> lock(sortSetPeriodically_mutex_);
// 遍历multiset
    for (auto it = reflectorPanelPoints_vector.begin(); it != reflectorPanelPoints_vector.end(); ) {
        if ((*it)->id == reflectorPanelPointId) {
            // 删除当前元素
            it = reflectorPanelPoints_vector.erase(it); // erase返回下一个有效的迭代器
        } else {
            ++it;
        }
    }
    // 唤醒等待的排序线程
    sortSetPeriodically_cond_var_.notify_one();
    std::cout<<MAGENTA<<"=========std::vector 删除数据 排序好的反光板数据 自动排序======reflectorPanelPoints_vector.size(): " <<reflectorPanelPoints_vector.size()<<RESET<<std::endl;
}
//删除所有数据
void deleteAllReflectorPanelPoints_vector(std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> & reflectorPanelPoints_vector) {
    iswirtortSetPeriodically_=true;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    std::lock_guard<std::mutex> lock(sortSetPeriodically_mutex_);
    reflectorPanelPoints_vector.clear();
    sortSetPeriodically_cond_var_.notify_one();
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    iswirtortSetPeriodically_=false;

}
//删除所有容器数据
void deleteAllContainerData(){
    // 加锁
    std::unique_lock<std::mutex> lock1(scancallback_mutex_);
    std::cout<<MAGENTA<<"111111111111111111111111"<<RESET<<std::endl;

    std::lock_guard<std::mutex> lock2(filePointMutex);//filePointMutex fileMutex
    std::cout<<MAGENTA<<"2222222222222222222222222"<<RESET<<std::endl;

    std::lock_guard<std::mutex> lock3(mapPointMutex);//mapPointMutex mapMutex
    std::cout<<MAGENTA<<"3333333333333333333333333333333333"<<RESET<<std::endl;

    std::unique_lock<std::mutex> lock4(queue_mutex_);
    std::cout<<MAGENTA<<"44444444444444444444444444444"<<RESET<<std::endl;

    std::lock_guard<std::mutex> lock5(m_node_mutex_);
    std::cout<<MAGENTA<<"5555555555555555555555555"<<RESET<<std::endl;

    //  std::unique_lock<std::mutex> lock6(sortSetPeriodically_mutex_); deleteAllReflectorPanelPoints_vector()中已经包含
    std::cout<<MAGENTA<<"666666666666666666666666666666666"<<RESET<<std::endl;


    //删除数据
    pointsSaveMap_.clear();
    std::cout<<MAGENTA<<"7777777777777777777777777"<<RESET<<std::endl;
    unorderedEdgeMapNew_.clear();
    std::cout<<MAGENTA<<"8888888888888888888888888888"<<RESET<<std::endl;
    unorderedEdgeMapNew_IdKey_.clear();
    std::cout<<MAGENTA<<"999999999999999999999999999999"<<RESET<<std::endl;
    while (iswileTure_){
        if(!iswileTure_){
            return;
        }
        if(!iswirtMatchokReflectoUnordered_map_){
            //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
            matchokReflectorPanelPointEnd_Unordered_map_.clear();
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            break;
        }
    }
    std::cout<<MAGENTA<<"1010101010101011010101010101100"<<RESET<<std::endl;
    deleteAllReflectorPanelPoints_vector(reflectorPanelPoints_vector_) ;
    std::cout<<MAGENTA<<"11 11 11 11 11"<<RESET<<std::endl;
    reflectorPanelPointAllMaps_.clear();
    std::cout<<MAGENTA<<"12 12 12 12 12 12 "<<RESET<<std::endl;
    reflectorPanelPointAllUnordered_maps_.clear();
    std::cout<<MAGENTA<<"13 13 13 13 13 13"<<RESET<<std::endl;
    ReflectorPanelPoint::deleteAllInstance();
    std::cout<<MAGENTA<<"14 14 14 14 14"<<RESET<<std::endl;
    std::cout<<"deleteAllInstance ReflectorPanelPoint::instances.size():"<<ReflectorPanelPoint::instances.size()<<std::endl;
}


//读取json文件 加载文件中的json数据，读取反光板点为数据存入 dataInfoMaps_
void loadAllPointJSONDataFromFile( std::string& filename, std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> &reflectorPanelPoints_vector,std::map<std::string ,std::shared_ptr<ReflectorPanelPoint>> &reflectorPanelPointAllMaps,std::unordered_map<std::tuple<double, double>, std::shared_ptr<ReflectorPanelPoint>, TupleHash, TupleEqual>  & reflectorPanelPointAllUnordered_maps) {

    //删除所有容器数据
    deleteAllContainerData();
    std::ifstream inputFile(filename);//读取数据
    int markerId=0;
    try{
        if (inputFile.is_open()) {
            std::string line;
            while (std::getline(inputFile, line)) {
                std::istringstream iss(line);
                std::cout<<"loadAllPointJSONDataFromFile: "<<std::endl;
                std::cout<<line<<std::endl;
                if(line.empty())
                    continue;
                //解析数据 到 dataInfoMaps_
                parseJSONDataWithAllPoint(line, reflectorPanelPoints_vector,reflectorPanelPointAllMaps,reflectorPanelPointAllUnordered_maps,markerId);

            }
            inputFile.close();
        } else {
            std::cerr << "Error: Unable to open file: " << filename << std::endl;
        }

    }catch(std::exception &e){

        if (inputFile.is_open())
            inputFile.close();

        std::cout<<"loadAllPointJSONDataFromFile: "<<e.what()<<std::endl;
    }

    queue_cond_var_.notify_one();
    sortSetPeriodically_cond_var_.notify_one();

}

//写入json文件 储存反光板数据到json
void savePosintsWithJsonFile(std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> &reflectorPanelPoints_vector) {
    std::cout << "----------------------------------------------------------"<<std::endl;
    iswirtortSetPeriodically_=true;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    std::cout << "写入json文件 储存反光板数据到json: reflectorPanelPoints_vector.size()" << reflectorPanelPoints_vector.size()<<std::endl;

    if(reflectorPanelPoints_vector.size()==0){
        std::cerr << "error Saving positions..." <<"reflectorPanelPoints_vector is empty" << std::endl;
        return;
    }
    std::ofstream outputFile(savePointFilePath_, std::ios::app);
    if (!outputFile.is_open()) {
        std::cerr << "Error: Unable to open file: " << calculateFilePath_ << std::endl;
        return;
    }
    for (const auto& pair : reflectorPanelPoints_vector) {
        std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint=pair;
        //生成json allpoint.txt
        std::string jsonData = generateJSONData_with_point( reflectorPanelPoint);
        std::cout << " jsonData:  " <<jsonData << std::endl;
        saveJSONDataToFile(outputFile,jsonData, savePointFilePath_);

    }
    outputFile.close();
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    iswirtortSetPeriodically_=false;

    std::cout << "savePosintsWithJsonFile reflectorPanelPoints_vector.size(): " <<reflectorPanelPoints_vector.size()<< std::endl;
}



//========================计算2个点连成的 edge 直线边长数据==============================================


//解析 计算好的反光板数据
void parseJSONDataWithCalculateEdge(const std::string& jsonData,
                                    const std::map<std::string ,std::shared_ptr<ReflectorPanelPoint>> & reflectorPanelPointAllMaps,
                                    std::unordered_map<std::string , std::vector<std::tuple<std::vector<double>, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>>> &unorderedEdgeMapNew,
                                    std::unordered_map<std::vector<std::string>, std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>,VectorStringHash, VectorStringEqual> &unorderedEdgeMapNew_IdKey
) {


    /**

    {
        "id": "xxxxx-xxxxx",
        "shape": "CYLINDER",
        "range": 360.0,
        "intensity": 0.05,
        "source_frame_id": "map",
        "target_frame_id": "base_link",
        "edge": 0.05,
        "point_first": ["ID1", "CYLINDER", 360.0, 0.025, "map", "base_link", 1.1, 1, 0, 0, 0, 0, 1],
        "point_second":["ID2","CYLINDER", 360.0, 0.025, "map", "base_link", 1.1, 1, 0, 0, 0, 0, 1]

    }
  */


    // 创建解析器对象
    Parser parser;

    // 加载 JSON 字符串
    parser.load(jsonData);
    // 解析 JSON 字符串
    Json json_data = parser.parse();
    std::cout << "------------------------------------parseJSONDataWithCalculateEdge----------------------------------------------------"  << std::endl;
    // 打印解析后的 JSON 对象
    std::cout << "解析数据 Parsed JSON data:\n" << json_data << std::endl;
    double edge=0.0;
    std::string id1=json_data["id"].isNull()?"EMPTY": json_data["id"].isString()?json_data["id"].asString():"";//唯一标示
    std::string shape =json_data["shape"].isNull()?"EMPTY": json_data["shape"].isString()?json_data["shape"].asString():"";
    double range = json_data["range"].isDouble()?json_data["range"].asDouble():json_data["range"].isInt()?json_data["range"].asInt():0;
    double intensity = json_data["intensity"].isDouble()?json_data["intensity"].asDouble():json_data["intensity"].isInt()?json_data["intensity"].asInt():0;
    std::string source_frame_id =json_data["source_frame_id"].isNull()?"EMPTY": json_data["source_frame_id"].isString()?json_data["source_frame_id"].asString():"EMPTY";
    std::string target_frame_id =json_data["target_frame_id"].isNull()?"EMPTY": json_data["target_frame_id"].isString()?json_data["target_frame_id"].asString():"EMPTY";
    edge = json_data["edge"].isDouble()?json_data["edge"].asDouble():json_data["edge"].isInt()?json_data["edge"].asInt():0;

    Json point_first =  json_data["point_first"];
    Json point_second =  json_data["point_second"];
    int aa =point_first.size();
    int aa1 =point_second.size();
    if(
            !point_first.isNull() && point_first.isArray() &&
            !point_second.isNull() && point_second.isArray() &&
            point_first.size()==13 &&
            point_first.size()==point_second.size()
            ){
        std::string id_first = point_first[0].isNull()?"EMPTY": point_first[0].isString()?point_first[0].asString():"";//唯一标示
        std::string id_second =point_second[0].isNull()?"EMPTY": point_second[0].isString()?point_second[0].asString():"";//唯一标示

        auto it_first = reflectorPanelPointAllMaps.find(id_first);
        auto it_second = reflectorPanelPointAllMaps.find(id_second);



        if (it_first == reflectorPanelPointAllMaps.end()|| it_second == reflectorPanelPointAllMaps.end()){
            std::cerr<<"reflectorPanelPointAllMaps 无数据"<<std::endl;
            return;
        }
        //ReflectorPanelPoint 是单例对象，直接从map中获取就行，或者   std::shared_ptr<ReflectorPanelPoint> point =  ReflectorPanelPoint::createInstance1(id)
        std::shared_ptr<ReflectorPanelPoint>  point1 =reflectorPanelPointAllMaps.at(id_first);
        std::shared_ptr<ReflectorPanelPoint>  point2 =reflectorPanelPointAllMaps.at(id_second);

        unorderedEdgeMapNew[id1].emplace_back(std::make_tuple(std::vector<double>{edge},std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>{{point1, point2}}));
        //   unorderedEdgeMapNew[id2].emplace_back(std::make_tuple(std::vector<double>{distance},std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>{{point2, point1}}));
        // std::vector<std::string>{point1->id, point2->id};
        unorderedEdgeMapNew_IdKey[std::vector<std::string>{point1->id, point2->id}]=std::make_tuple(edge,std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>{{point1, point2}});



    }//   if(!point_first.isNull()&&!point_first.isArray()&&!point_first.isNull()&&!point_first.isArray()&&point_first.size()==13&&point_first.size()==point_second.size())

    // json_data.clear();//释放内存
}


//读取json文件 加载文件中的json数据，读取反光板点为数据存入 dataInfoMaps_
void loadCalculateEdgeJSONDataFromFile( std::string& filename,
                                        const std::map<std::string ,std::shared_ptr<ReflectorPanelPoint>> & reflectorPanelPointAllMaps,
                                        std::unordered_map<std::string , std::vector<std::tuple<std::vector<double>, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>>> &unorderedEdgeMapNew,
                                        std::unordered_map<std::vector<std::string>, std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>,VectorStringHash, VectorStringEqual> &unorderedEdgeMapNew_IdKey
) {
    // 加锁
    //std::lock_guard<std::mutex> lock(filePointMutex);//filePointMutex fileMutex
    //  std::lock_guard<std::mutex> lock1(mapPointMutex);//mapPointMutex mapMutex
    unorderedEdgeMapNew.clear();//先清空数据
    unorderedEdgeMapNew_IdKey.clear();//先清空数据
    std::ifstream inputFile(filename);//读取数据
    try{
        if (inputFile.is_open()) {
            std::string line;
            while (std::getline(inputFile, line)) {
                std::istringstream iss(line);
                std::cout<<"loadCalculateEdgeJSONDataFromFile"<<std::endl;

                std::cout<<line<<std::endl;
                if(line.empty())
                    continue;

                std::cout<<"loadCalculateEdgeJSONDataFromFile"<<std::endl;
                //解析数据 到 dataInfoMaps_
                parseJSONDataWithCalculateEdge(line, reflectorPanelPointAllMaps,unorderedEdgeMapNew,unorderedEdgeMapNew_IdKey);

            }
            inputFile.close();
        } else {
            std::cerr << "Error: Unable to open file: " << filename << std::endl;
        }

    }catch(std::exception &e){

        if (inputFile.is_open())
            inputFile.close();

        std::cerr<<"loadCalculateEdgeJSONDataFromFile: "<<e.what()<<std::endl;
    }

}


//封装计算好的反光板特征json数据
std::string generateJSONData_with_json_new(double edge,
                                           const std::shared_ptr<ReflectorPanelPoint> &point1,
                                           const std::shared_ptr<ReflectorPanelPoint> &point2) {
    std::cout<<" start======================封装json数据 generateJSONData_with_json_new================================="<<std::endl;

    /**

    {
        "id": "xxxxx-xxxxx",
        "shape": "CYLINDER",
        "range": 360.0,
        "intensity": 0.05,
        "source_frame_id": "map",
        "target_frame_id": "base_link",
        "edge": 0.05,
        "point_first": ["ID1", "CYLINDER", 360.0, 0.025, "map", "base_link", 1.1, 1, 0, 0, 0, 0, 1],
        "point_second":["ID2","CYLINDER", 360.0, 0.025, "map", "base_link", 1.1, 1, 0, 0, 0, 0, 1]

    }
*/


    Json json_data;
    json_data["id"]=point1->id;
    json_data["shape"]=point1->shape;
    json_data["range"]=point1->range;
    json_data["intensity"]=point1->intensity;
    json_data["source_frame_id"]=point1->source_frame_id;
    json_data["target_frame_id"]=point1->target_frame_id;
    json_data["edge"]  =edge;
    Json item_first;
    item_first.append(point1->id);
    item_first.append(point1->shape);
    item_first.append(point1->range);
    item_first.append(point1->intensity);
    item_first.append(point1->source_frame_id);
    item_first.append(point1->target_frame_id);
    item_first.append(point1->x);
    item_first.append(point1->y);
    item_first.append(point1->z);
    item_first.append(point1->rx);
    item_first.append(point1->ry);
    item_first.append(point1->rz);
    item_first.append(point1->rw);

    Json item_second;
    item_second.append(point2->id);
    item_second.append(point2->shape);
    item_second.append(point2->range);
    item_second.append(point2->intensity);
    item_second.append(point2->source_frame_id);
    item_second.append(point2->target_frame_id);
    item_second.append(point2->x);
    item_second.append(point2->y);
    item_second.append(point2->z);
    item_second.append(point2->rx);
    item_second.append(point2->ry);
    item_second.append(point2->rz);
    item_second.append(point2->rw);
    json_data["point_first"]=item_first;
    json_data["point_second"]=item_second;

    std::string   jsonData=json_data.str();
    json_data.clear();//用完,内存释放

    std::cout<<"generateJSONData_with_json_new: "<<jsonData<<std::endl;
    return jsonData;
}

// 计算2个点连成的 edge 直线边长数据,并保存数据到json文件
void calculateEdge(
        const   std::multiset<std::shared_ptr<ReflectorPanelPoint>,CompareByDistanceAndX> &reflectorPanelPoints_vector,
        std::unordered_map<std::string , std::vector<std::tuple<std::vector<double>, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>>> &unorderedEdgeMapNew,
        std::unordered_map<std::vector<std::string>, std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>,VectorStringHash, VectorStringEqual> &unorderedEdgeMapNew_IdKey


) {
    std::ofstream outputFile(calculateFilePath_, std::ios::app);
    if (!outputFile.is_open()) {
        std::cerr << "Error: Unable to open file: " << calculateFilePath_ << std::endl;
        return;
    }

    //计算 edge 边
    for (const auto& pair1 : reflectorPanelPoints_vector) {//第一个点

        if (!pair1) {
            std::cerr << "pair1 empty "  << std::endl;
            continue;
        }

        const std::string& id1 = pair1->id;
        std::shared_ptr<ReflectorPanelPoint> point1 = pair1;

        for (const auto& pair2 : reflectorPanelPoints_vector) {//第二个点

            if (!pair2) {  //操作这里的数据时关闭自动排序，否则错
                std::cerr << "pair2 empty "  << std::endl;
                continue;
                // return;
            }
            //todo 有空指针问题
            if (id1 != pair2->id) { // 确保不重复计算(id30,id30)
                std::cerr << "1.1Error:calculateEdge edge: "  << std::endl;
                const std::string& id2 = pair2->id;
                std::cerr << "1.21Error:calculateEdge edge: " << std::endl;
                std::shared_ptr<ReflectorPanelPoint> point2 = pair2;
                std::cerr << "1.3Error:calculateEdge edge: " << std::endl;
                // 计算距离
                double edge = calculateDistance(*point1, *point2);
                std::cerr << "2.Error:calculateEdge edge: " << edge << std::endl;

                //calculateEdge_threshold_ 计算特征时的距离阀值，计算距离阀值，两点过远就不需要计算了
                if(edge>=calculateEdge_threshold_)
                    continue;



                //转json
                std::string jsonData =  generateJSONData_with_json_new(edge,point1,point2);
                std::cerr << "4.Error:calculateEdge jsonData: " << jsonData << std::endl;
                // 组织数据结构
                unorderedEdgeMapNew[id1].emplace_back(std::make_tuple(std::vector<double>{edge},std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>{{point1, point2}}));

                std::cerr << "5.Error:calculateEdge jsonData: " << jsonData << std::endl;
                //   unorderedEdgeMapNew[id2].emplace_back(std::make_tuple(std::vector<double>{distance},std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>{{point2, point1}}));
                // std::vector<std::string>{point1->id, point2->id};
                unorderedEdgeMapNew_IdKey[std::vector<std::string>{point1->id, point2->id}]=std::make_tuple(edge,std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>{{point1, point2}});
                std::cerr << "6.Error:calculateEdge edge: " << edge << std::endl;
                //写入txt数据
                saveJSONDataToFile(outputFile,jsonData, calculateFilePath_);
                std::cerr << "7.Error:calculateEdge edge: " << edge << std::endl;

            }
        }
    }
    std::cerr << "100.Error:calculateEdge edge: "  << std::endl;
    outputFile.close();

}//void calculateEdge
//--------------------------------------------------------------------------
//=========================================================================
//--------------------------------------------
//------------------------------------------------
//发布性能评估数据 /trilateration_time_log
void publishTrilaterationLog(
        const int reflectorPanelPoints_size,
        const double duration_count,
        const std::string& resultType,
        const std::string& resultChineseMessage,
        const std::string& resultENMessage,
        const std::string& className,
        int   classLine,
        const int time_log_fig,
        const int matchokReflectorPanelPointSize,
        const int reflectorPanelPointAllUnordered_maps_size,
        const string &matchReflectorPaneIds,
        const geometry_msgs::Pose &mapTbase_link,
        double &mapTbase_link_yaw,
        bool &computeCurrentPoseFig,
        double &computeCurrentPose_score //当前匹配分数

) {

    fanguangbantrilateration::trilateration_read_time_log trilateration_read_time_log2;
    trilateration_read_time_log2.result_type = resultType;
    trilateration_read_time_log2.result_chinese_message = resultChineseMessage;
    trilateration_read_time_log2.result_EN_message = resultENMessage;
    trilateration_read_time_log2.className = className;
    trilateration_read_time_log2.classLine = classLine;
    trilateration_read_time_log2.num_points = reflectorPanelPoints_size;
    trilateration_read_time_log2.execution_time =std::to_string(duration_count) ;
    trilateration_read_time_log2.time_log_fig=time_log_fig;

    trilateration_read_time_log2.computeCurrentPose_score=computeCurrentPose_score;//预估机器人位置得分
    trilateration_read_time_log2.score_min_threshold=score_min_threshold_;//预估机器人位置得分下限
    trilateration_read_time_log2.score_max_threshold=score_max_threshold_;//预估机器人位置得分上限
    trilateration_read_time_log2.computeCurrentPoseFig=computeCurrentPoseFig;//true 预估机器人成功

    trilateration_read_time_log2.num_points=reflectorPanelPointAllUnordered_maps_size;//匹配上的数据
    trilateration_read_time_log2.matchokReflectorPanelPointSize=matchokReflectorPanelPointSize;//队列数量
    trilateration_read_time_log2.matchReflectorPaneIds=matchReflectorPaneIds;//匹配上的反光板ids
    trilateration_read_time_log2.mapTbase_link=mapTbase_link;//计算得到的移动机器人位置
    trilateration_read_time_log2.mapTbase_link_yaw=mapTbase_link_yaw;//算得到的移动机器人姿态
    time_log_pub_.publish(trilateration_read_time_log2);
}

//------------------------------------------------
//发布性能评估数据 /trilateration_time_log
void publishTrilaterationLog(
        const int reflectorPanelPoints_size,
        const double duration_count,
        const std::string& resultType,
        const std::string& resultChineseMessage,
        const std::string& resultENMessage,
        const std::string& className,
        int   classLine,
        const int time_log_fig,
        const int matchokReflectorPanelPointSize,
        const int reflectorPanelPointAllUnordered_maps_size,
        const string &matchReflectorPaneIds,
        const geometry_msgs::Pose &mapTbase_link,
        double &mapTbase_link_yaw

) {

/*

    选择性能评估数据   /trilateration_time_log
    int time_log_fig_=-1;
    -1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。
    1：FindCircle: 通过雷达强度数据，使用三角函数拟定圆心坐标，并存入反光板数据缓存中
    2：匹配数据: 通过得到的圆心坐标，在已经匹配成功的反光板数据缓存中匹配数据
    3：匹配数据: 通过得到的圆心坐标，在储存所有的反光板数据缓存中匹配数据
    4：匹配数据: 通过得到的圆心坐标，在反光板数据缓存中匹配数据
    5：匹配数据: 匹配走广度优先算法 或 匹配走广度优先算法 + 优先使用匹配好的反光板数据
    6：匹配算法：从查找圆心到匹配完成需要的总执行时间
    7：位姿推算: 基于之前已经匹配上了反光板，推算位姿
    8：位姿推算: 经典三边定位最小二乘法，推算位姿
    9：位姿推算: 基于之前已经匹配上了反光板 + 经典三边定位最小二乘法，推算位姿
    10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
    11：匹配算法: 反光柱匹配算法：只用特征匹配算法  需要匹配的边长数量
    12：上一帧的位姿odomTbaselink与当前帧的位姿odomTbaselink之间的差异
*/

    fanguangbantrilateration::trilateration_read_time_log trilateration_read_time_log2;
    trilateration_read_time_log2.result_type = resultType;
    trilateration_read_time_log2.result_chinese_message = resultChineseMessage;
    trilateration_read_time_log2.result_EN_message = resultENMessage;
    trilateration_read_time_log2.className = className;
    trilateration_read_time_log2.classLine = classLine;
    trilateration_read_time_log2.num_points = reflectorPanelPoints_size;
    trilateration_read_time_log2.execution_time =std::to_string(duration_count) ;
    trilateration_read_time_log2.time_log_fig=time_log_fig;
    trilateration_read_time_log2.num_points=reflectorPanelPointAllUnordered_maps_size;//匹配上的数据
    trilateration_read_time_log2.matchokReflectorPanelPointSize=matchokReflectorPanelPointSize;//队列数量
    trilateration_read_time_log2.matchReflectorPaneIds=matchReflectorPaneIds;//匹配上的反光板ids
    trilateration_read_time_log2.mapTbase_link=mapTbase_link;//计算得到的移动机器人位置
    trilateration_read_time_log2.mapTbase_link_yaw=mapTbase_link_yaw;//算得到的移动机器人姿态
    time_log_pub_.publish(trilateration_read_time_log2);
}

//------------------------------------------------



//发布性能评估数据 /trilateration_time_log
void publishTrilaterationLog(

        const double duration_count,
        const std::string& resultType,
        const std::string& resultChineseMessage,
        const std::string& resultENMessage,
        const std::string& className,
        int   classLine,
        const int time_log_fig,
        const  tf::StampedTransform& previous_odomTbase_transform,//上一帧的位姿odomTbaselink
        const  tf::StampedTransform& predicted_odomTbase_transform,//当前的位姿odomTbaselink
        double &diff_previous_with_predicted_odomTbase_link_x,// 计算位移x
        double &diff_previous_with_predicted_odomTbase_link_y,// 计算位移y
        double &diff_previous_with_predicted_odomTbase_link_yaw,// 计算旋转角度差
        double &previous_with_predicted_distance,//距离差,欧式距离 distance_moved = std::sqrt(计算位移x * 计算位移x + 计算位移y * 计算位移y);
        double &previous_with_predicted_distance_threshold,//欧式距离 阀值，判断是否存在位移
        double &previous_with_predicted_distance_angle_threshold//角度 阀值，判断是否存在旋转

) {

/*

    选择性能评估数据   /trilateration_time_log
    int time_log_fig_=-1;
    -1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。
    1：FindCircle: 通过雷达强度数据，使用三角函数拟定圆心坐标，并存入反光板数据缓存中
    2：匹配数据: 通过得到的圆心坐标，在已经匹配成功的反光板数据缓存中匹配数据
    3：匹配数据: 通过得到的圆心坐标，在储存所有的反光板数据缓存中匹配数据
    4：匹配数据: 通过得到的圆心坐标，在反光板数据缓存中匹配数据
    5：匹配数据: 匹配走广度优先算法 或 匹配走广度优先算法 + 优先使用匹配好的反光板数据
    6：匹配算法：从查找圆心到匹配完成需要的总执行时间
    7：位姿推算: 基于之前已经匹配上了反光板，推算位姿
    8：位姿推算: 经典三边定位最小二乘法，推算位姿
    9：位姿推算: 基于之前已经匹配上了反光板 + 经典三边定位最小二乘法，推算位姿
   11：匹配算法: 反光柱匹配算法：只用特征匹配算法  需要匹配的边长数量
   12：位姿推算: 上一帧的位姿odomTbaselink与当前帧的位姿odomTbaselink之间的差异

 */

    //1. 创建一个上一帧的位姿odomTbaselink geometry_msgs::previous_pose 对象
    geometry_msgs::Pose previous_pose;
    // 提取平移信息并赋值给 pose.position
    previous_pose.position.x = previous_odomTbase_transform.getOrigin().x();
    previous_pose.position.y = previous_odomTbase_transform.getOrigin().y();
    previous_pose.position.z = previous_odomTbase_transform.getOrigin().z();
    // 提取旋转信息并赋值给 pose.orientation
    tf::Quaternion previous_quaternion = previous_odomTbase_transform.getRotation();
    previous_pose.orientation.x = previous_quaternion.x();
    previous_pose.orientation.y = previous_quaternion.y();
    previous_pose.orientation.z = previous_quaternion.z();
    previous_pose.orientation.w = previous_quaternion.w();

    //2. 创建一个当前的位姿odomTbaselink geometry_msgs::previous_pose 对象
    geometry_msgs::Pose predicted_pose;
    // 提取平移信息并赋值给 pose.position
    predicted_pose.position.x = predicted_odomTbase_transform.getOrigin().x();
    predicted_pose.position.y = predicted_odomTbase_transform.getOrigin().y();
    predicted_pose.position.z = predicted_odomTbase_transform.getOrigin().z();
    // 提取旋转信息并赋值给 pose.orientation
    tf::Quaternion predicted_quaternion = predicted_odomTbase_transform.getRotation();
    predicted_pose.orientation.x = predicted_quaternion.x();
    predicted_pose.orientation.y = predicted_quaternion.y();
    predicted_pose.orientation.z = predicted_quaternion.z();
    predicted_pose.orientation.w = predicted_quaternion.w();


    fanguangbantrilateration::trilateration_read_time_log trilateration_read_time_log2;
    trilateration_read_time_log2.result_type = resultType;
    trilateration_read_time_log2.result_chinese_message = resultChineseMessage;
    trilateration_read_time_log2.result_EN_message = resultENMessage;
    trilateration_read_time_log2.className = className;
    trilateration_read_time_log2.classLine = classLine;
    trilateration_read_time_log2.execution_time =std::to_string(duration_count) ;
    trilateration_read_time_log2.time_log_fig=time_log_fig;
    trilateration_read_time_log2.previous_odomTbase_link=previous_pose;//上一帧的位姿odomTbaselink
    trilateration_read_time_log2.previous_odomTbase_link_yaw= tf::getYaw(previous_quaternion);

    trilateration_read_time_log2.predicted_odomTbase_link=predicted_pose;//当前的位姿odomTbaselink
    trilateration_read_time_log2.predicted_odomTbase_link_yaw=tf::getYaw(predicted_quaternion);

    trilateration_read_time_log2.diff_previous_with_predicted_odomTbase_link_x=diff_previous_with_predicted_odomTbase_link_x;// 计算位移x
    trilateration_read_time_log2.diff_previous_with_predicted_odomTbase_link_y=diff_previous_with_predicted_odomTbase_link_y;// 计算位移y
    trilateration_read_time_log2.diff_previous_with_predicted_odomTbase_link_yaw=diff_previous_with_predicted_odomTbase_link_yaw;// 计算旋转角度差
    trilateration_read_time_log2.previous_with_predicted_distance=previous_with_predicted_distance,//欧式距离 distance_moved = std::sqrt(计算位移x * 计算位移x + 计算位移y * 计算位移y);
            trilateration_read_time_log2.previous_with_predicted_distance_threshold=previous_with_predicted_distance_threshold;//距离差,欧式距离 阀值，判断是否存在位移
    trilateration_read_time_log2.previous_with_predicted_distance_angle_threshold=previous_with_predicted_distance_angle_threshold;//角度 阀值，判断是否存在旋转
    time_log_pub_.publish(trilateration_read_time_log2);
}
//----------------------------------------------------------------

//发布性能评估数据 /trilateration_time_log
void publishTrilaterationLog(

        const double duration_count,
        const std::string& resultType,
        const std::string& resultChineseMessage,
        const std::string& resultENMessage,
        const std::string& className,
        int   classLine,
        const int time_log_fig,
        const  tf::Transform& previous_odomTbase_transform,//上一帧的位姿odomTbaselink
        const  tf::Transform& predicted_odomTbase_transform,//当前的位姿odomTbaselink
        double &diff_previous_with_predicted_odomTbase_link_x,// 计算位移x
        double &diff_previous_with_predicted_odomTbase_link_y,// 计算位移y
        double &diff_previous_with_predicted_odomTbase_link_yaw,// 计算旋转角度差
        double &previous_with_predicted_distance,//距离差,欧式距离 distance_moved = std::sqrt(计算位移x * 计算位移x + 计算位移y * 计算位移y);
        double &previous_with_predicted_distance_threshold,//欧式距离 阀值，判断是否存在位移
        double &previous_with_predicted_distance_angle_threshold//角度 阀值，判断是否存在旋转

) {

/*

    选择性能评估数据   /trilateration_time_log
    int time_log_fig_=-1;
    -1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。
    1：FindCircle: 通过雷达强度数据，使用三角函数拟定圆心坐标，并存入反光板数据缓存中
    2：匹配数据: 通过得到的圆心坐标，在已经匹配成功的反光板数据缓存中匹配数据
    3：匹配数据: 通过得到的圆心坐标，在储存所有的反光板数据缓存中匹配数据
    4：匹配数据: 通过得到的圆心坐标，在反光板数据缓存中匹配数据
    5：匹配数据: 匹配走广度优先算法 或 匹配走广度优先算法 + 优先使用匹配好的反光板数据
    6：匹配算法：从查找圆心到匹配完成需要的总执行时间
    7：位姿推算: 基于之前已经匹配上了反光板，推算位姿
    8：位姿推算: 经典三边定位最小二乘法，推算位姿
    9：位姿推算: 基于之前已经匹配上了反光板 + 经典三边定位最小二乘法，推算位姿
   11：匹配算法: 反光柱匹配算法：只用特征匹配算法  需要匹配的边长数量
   12：位姿推算: 上一帧的位姿odomTbaselink与当前帧的位姿odomTbaselink之间的差异

 */

    //1. 创建一个上一帧的位姿odomTbaselink geometry_msgs::previous_pose 对象
    geometry_msgs::Pose previous_pose;
    // 提取平移信息并赋值给 pose.position
    previous_pose.position.x = previous_odomTbase_transform.getOrigin().x();
    previous_pose.position.y = previous_odomTbase_transform.getOrigin().y();
    previous_pose.position.z = previous_odomTbase_transform.getOrigin().z();
    // 提取旋转信息并赋值给 pose.orientation
    tf::Quaternion previous_quaternion = previous_odomTbase_transform.getRotation();
    previous_pose.orientation.x = previous_quaternion.x();
    previous_pose.orientation.y = previous_quaternion.y();
    previous_pose.orientation.z = previous_quaternion.z();
    previous_pose.orientation.w = previous_quaternion.w();

    //2. 创建一个当前的位姿odomTbaselink geometry_msgs::previous_pose 对象
    geometry_msgs::Pose predicted_pose;
    // 提取平移信息并赋值给 pose.position
    predicted_pose.position.x = predicted_odomTbase_transform.getOrigin().x();
    predicted_pose.position.y = predicted_odomTbase_transform.getOrigin().y();
    predicted_pose.position.z = predicted_odomTbase_transform.getOrigin().z();
    // 提取旋转信息并赋值给 pose.orientation
    tf::Quaternion predicted_quaternion = predicted_odomTbase_transform.getRotation();
    predicted_pose.orientation.x = predicted_quaternion.x();
    predicted_pose.orientation.y = predicted_quaternion.y();
    predicted_pose.orientation.z = predicted_quaternion.z();
    predicted_pose.orientation.w = predicted_quaternion.w();


    fanguangbantrilateration::trilateration_read_time_log trilateration_read_time_log2;
    trilateration_read_time_log2.result_type = resultType;
    trilateration_read_time_log2.result_chinese_message = resultChineseMessage;
    trilateration_read_time_log2.result_EN_message = resultENMessage;
    trilateration_read_time_log2.className = className;
    trilateration_read_time_log2.classLine = classLine;
    trilateration_read_time_log2.execution_time =std::to_string(duration_count) ;
    trilateration_read_time_log2.time_log_fig=time_log_fig;
    trilateration_read_time_log2.previous_odomTbase_link=previous_pose;//上一帧的位姿odomTbaselink
    trilateration_read_time_log2.previous_odomTbase_link_yaw= tf::getYaw(previous_quaternion);

    trilateration_read_time_log2.predicted_odomTbase_link=predicted_pose;//当前的位姿odomTbaselink
    trilateration_read_time_log2.predicted_odomTbase_link_yaw=tf::getYaw(predicted_quaternion);

    trilateration_read_time_log2.diff_previous_with_predicted_odomTbase_link_x=diff_previous_with_predicted_odomTbase_link_x;// 计算位移x
    trilateration_read_time_log2.diff_previous_with_predicted_odomTbase_link_y=diff_previous_with_predicted_odomTbase_link_y;// 计算位移y
    trilateration_read_time_log2.diff_previous_with_predicted_odomTbase_link_yaw=diff_previous_with_predicted_odomTbase_link_yaw;// 计算旋转角度差
    trilateration_read_time_log2.previous_with_predicted_distance=previous_with_predicted_distance,//欧式距离 distance_moved = std::sqrt(计算位移x * 计算位移x + 计算位移y * 计算位移y);
            trilateration_read_time_log2.previous_with_predicted_distance_threshold=previous_with_predicted_distance_threshold;//距离差,欧式距离 阀值，判断是否存在位移
    trilateration_read_time_log2.previous_with_predicted_distance_angle_threshold=previous_with_predicted_distance_angle_threshold;//角度 阀值，判断是否存在旋转
    time_log_pub_.publish(trilateration_read_time_log2);
}
//------------------------------------------------------------------------------------


//发布性能评估数据 /trilateration_time_log
void publishTrilaterationLog(

        const double duration_count,
        const std::string& resultType,
        const std::string& resultChineseMessage,
        const std::string& resultENMessage,
        const std::string& className,
        int   classLine,
        const int time_log_fig,
        const  tf::Transform& calculate_mapTbase_link,//反光柱计算得到mapTbase_link
        const  tf::Transform& predicted_mapTbase_link,//当前的位姿tf 得到mapTbase_link
        const  tf::Transform& previous_odomTbase_transform,//上一帧的位姿odomTbaselink
        const  tf::Transform& predicted_odomTbase_transform,//当前的位姿odomTbaselink
        double &diff_variancePose_min,//  计算位置方差
        double &diff_varianceYaw_min,//  计算角度方差
        double &variance_min_pose_lerance,//角度方差阀值
        double &variance_min_yaw_lerance,//位置方差阀值
        const int optimizePoseWithTrilaterationPose_queue_size//队列数量


) {
    //==========================反光柱计算得到mapTbase_link=================================
    //1. 创建反光柱计算得到mapTbase_link 的位姿odomTbaselink geometry_msgs::previous_pose 对象
    geometry_msgs::Pose calculate_mapTbase_link_pose;
    // 提取平移信息并赋值给 pose.position
    calculate_mapTbase_link_pose.position.x = calculate_mapTbase_link.getOrigin().x();
    calculate_mapTbase_link_pose.position.y = calculate_mapTbase_link.getOrigin().y();
    calculate_mapTbase_link_pose.position.z = calculate_mapTbase_link.getOrigin().z();
    // 提取旋转信息并赋值给 pose.orientation
    tf::Quaternion calculate_mapTbase_link_pose_quaternion = calculate_mapTbase_link.getRotation();
    calculate_mapTbase_link_pose.orientation.x = calculate_mapTbase_link_pose_quaternion.x();
    calculate_mapTbase_link_pose.orientation.y = calculate_mapTbase_link_pose_quaternion.y();
    calculate_mapTbase_link_pose.orientation.z = calculate_mapTbase_link_pose_quaternion.z();
    calculate_mapTbase_link_pose.orientation.w = calculate_mapTbase_link_pose_quaternion.w();

    //2. 创建一个当前的位姿odomTbaselink geometry_msgs::previous_pose 对象
    geometry_msgs::Pose predicted_mapTbase_link_pose;
    // 提取平移信息并赋值给 pose.position
    predicted_mapTbase_link_pose.position.x = predicted_mapTbase_link.getOrigin().x();
    predicted_mapTbase_link_pose.position.y = predicted_mapTbase_link.getOrigin().y();
    predicted_mapTbase_link_pose.position.z = predicted_mapTbase_link.getOrigin().z();
    // 提取旋转信息并赋值给 pose.orientation
    tf::Quaternion predicted_mapTbase_link_quaternion = predicted_mapTbase_link.getRotation();
    predicted_mapTbase_link_pose.orientation.x = predicted_mapTbase_link_quaternion.x();
    predicted_mapTbase_link_pose.orientation.y = predicted_mapTbase_link_quaternion.y();
    predicted_mapTbase_link_pose.orientation.z = predicted_mapTbase_link_quaternion.z();
    predicted_mapTbase_link_pose.orientation.w = predicted_mapTbase_link_quaternion.w();


    //==========================odomTbase======================================
    //1. 创建一个上一帧的位姿odomTbaselink geometry_msgs::previous_pose 对象
    geometry_msgs::Pose previous_pose;
    // 提取平移信息并赋值给 pose.position
    previous_pose.position.x = previous_odomTbase_transform.getOrigin().x();
    previous_pose.position.y = previous_odomTbase_transform.getOrigin().y();
    previous_pose.position.z = previous_odomTbase_transform.getOrigin().z();
    // 提取旋转信息并赋值给 pose.orientation
    tf::Quaternion previous_quaternion = previous_odomTbase_transform.getRotation();
    previous_pose.orientation.x = previous_quaternion.x();
    previous_pose.orientation.y = previous_quaternion.y();
    previous_pose.orientation.z = previous_quaternion.z();
    previous_pose.orientation.w = previous_quaternion.w();

    //2. 创建一个当前的位姿odomTbaselink geometry_msgs::previous_pose 对象
    geometry_msgs::Pose predicted_pose;
    // 提取平移信息并赋值给 pose.position
    predicted_pose.position.x = predicted_odomTbase_transform.getOrigin().x();
    predicted_pose.position.y = predicted_odomTbase_transform.getOrigin().y();
    predicted_pose.position.z = predicted_odomTbase_transform.getOrigin().z();
    // 提取旋转信息并赋值给 pose.orientation
    tf::Quaternion predicted_quaternion = predicted_odomTbase_transform.getRotation();
    predicted_pose.orientation.x = predicted_quaternion.x();
    predicted_pose.orientation.y = predicted_quaternion.y();
    predicted_pose.orientation.z = predicted_quaternion.z();
    predicted_pose.orientation.w = predicted_quaternion.w();


    fanguangbantrilateration::trilateration_read_time_log trilateration_read_time_log2;
    trilateration_read_time_log2.result_type = resultType;
    trilateration_read_time_log2.result_chinese_message = resultChineseMessage;
    trilateration_read_time_log2.result_EN_message = resultENMessage;
    trilateration_read_time_log2.className = className;
    trilateration_read_time_log2.classLine = classLine;
    trilateration_read_time_log2.execution_time =std::to_string(duration_count) ;
    trilateration_read_time_log2.time_log_fig=time_log_fig;
    //队列数量
    trilateration_read_time_log2.optimizePoseWithTrilaterationPose_queue_size=optimizePoseWithTrilaterationPose_queue_size;


    trilateration_read_time_log2.diff_variancePose_min=diff_variancePose_min;
    trilateration_read_time_log2.diff_varianceYaw_min=diff_varianceYaw_min;
    trilateration_read_time_log2.variance_min_pose_lerance=variance_min_pose_lerance;
    trilateration_read_time_log2.variance_min_yaw_lerance=variance_min_yaw_lerance;
    trilateration_read_time_log2.isTrilateration_with_yawFig=isTrilateration_with_yawFig_;
    trilateration_read_time_log2.firstTrilateration_with_yawFig=firstTrilateration_with_yawFig_;

    //-----------------------------------------------
    trilateration_read_time_log2.previous_odomTbase_link=previous_pose;//上一帧的位姿odomTbaselink
    trilateration_read_time_log2.previous_odomTbase_link_yaw= tf::getYaw(previous_quaternion);

    trilateration_read_time_log2.predicted_odomTbase_link=predicted_pose;//当前的位姿odomTbaselink
    trilateration_read_time_log2.predicted_odomTbase_link_yaw=tf::getYaw(predicted_quaternion);
    //--------------------------------------------------

    trilateration_read_time_log2.calculate_mapTbase_link=calculate_mapTbase_link_pose;//反光柱计算得到的mapTbase_link
    trilateration_read_time_log2.calculate_mapTbase_link_yaw= tf::getYaw(calculate_mapTbase_link_pose_quaternion);

    trilateration_read_time_log2.predicted_mapTbase_link=predicted_mapTbase_link_pose;//当前tf得到的位姿mapTbase_link
    trilateration_read_time_log2.predicted_mapTbase_link_yaw=tf::getYaw(predicted_mapTbase_link_quaternion);

    //----------------------------------------------------
    time_log_pub_.publish(trilateration_read_time_log2);
}



//=============================获取雷达数据后使用的========================================
// 计算目标多边形的边长
std::vector<double> calculateTargetEdges(const std::vector<Point>& points) {
    // 计算边长 对于每个点 points1[i]，计算其与下一个点 points1[next_i] 之间的距离，targetEdges 中最后一个边长是由最后一个点 points1[points1.size() - 1] 和第一个点 points1[0] 构成的。
    std::vector<double> targetEdges;
    ROS_INFO_STREAM("=============================计算目标多边形的边长 calculateTargetEdges 打印点坐标 , 点数量points.size()"<<points.size());
    for (size_t i = 0; i < points.size(); ++i) {
        size_t next_i = (i + 1) % points.size();
        //打印点坐标
        ROS_INFO_STREAM("-----------------------------------------------calculateTargetEdges--------------------------------------------------------------------------)");
        ROS_INFO_STREAM(" 打印当前坐标 Point " << i << ": (" << points[i].x << ", " << points[i].y << ")");
        ROS_INFO_STREAM(" 打印下一个坐标 Point " << next_i << ": (" << points[next_i].x << ", " << points[next_i].y << ")");
        double distance = calculateDistance(points[i], points[next_i]);
        targetEdges.push_back(distance);
    }
    return targetEdges;
}

// 计算反光板组成的多边形的角度特征，目标多边形的角度
std::vector<double> calculateTargetAngles(const std::vector<Point>& points) {
    // 计算角度 targetAngles 中的第一个夹角，如果当前点是第一个点，则前一个点是最后一个点。（ 第一个夹角：  前一个点-当前点-最后一个点 ）
    std::vector<double> targetAngles;
    ROS_INFO_STREAM("============================= 目标多边形的角度 calculateTargetAngles 打印点坐标 , 点数量points.size()"<<points.size());
    for (size_t i = 0; i < points.size(); ++i) {
        size_t prev_i = (i == 0) ? points.size() - 1 : i - 1;
        size_t next_i = (i + 1) % points.size();
        //打印点坐标
        ROS_INFO_STREAM("------------------------------------------calculateTargetAngles-------------------------------------------------------------------------------)");
        ROS_INFO_STREAM("打印当前坐标 Point " << i << ": (" << points[i].x << ", " << points[i].y << ")");
        ROS_INFO_STREAM(" 打印下一个坐标 Point " << next_i << ": (" << points[next_i].x << ", " << points[next_i].y << ")");
        double angle = calculateAngle(points[prev_i], points[i], points[next_i]);
        targetAngles.push_back(angle);
    }
    return targetAngles;
}
//-------------------------------------------------------------------------------

// 计算目标多边形的边长
bool calculateTargetEdges(const std::vector<Point>& points,std::vector<double> &result_targetEdges_vector) {
    // 计算边长 对于每个点 points1[i]，计算其与下一个点 points1[next_i] 之间的距离，targetEdges 中最后一个边长是由最后一个点 points1[points1.size() - 1] 和第一个点 points1[0] 构成的。
    ROS_INFO_STREAM("=============================计算目标多边形的边长 calculateTargetEdges 打印点坐标 , 点数量points.size()"<<points.size());
    try{
        for (size_t i = 0; i < points.size(); ++i) {
            size_t next_i = (i + 1) % points.size();
            //打印点坐标
            ROS_INFO_STREAM("-----------------------------------------------calculateTargetEdges--------------------------------------------------------------------------)");
            ROS_INFO_STREAM(" 打印当前坐标 Point " << i << ": (" << points[i].x << ", " << points[i].y << ")");
            ROS_INFO_STREAM(" 打印下一个坐标 Point " << next_i << ": (" << points[next_i].x << ", " << points[next_i].y << ")");
            double distance = calculateDistance(points[i], points[next_i]);
            if (    std::isnan(distance) || std::isinf(distance)) {
                std::cout << "计算目标多边形的边长 calculateTargetEdges edge is a valid isnan or finite number.不能加入边长计算。" << std::endl;
                return false;
            }
            result_targetEdges_vector.push_back(distance);
        }
    }catch (std::exception e) {
        ROS_INFO_STREAM("ERROR 计算目标多边形的边长 calculateTargetEdges error:"<<e.what());
        return false;
    }


    return true;
}

//---------------------------------------------------------------------------------

// 计算目标多边形的边长
bool calculateTargetEdges(const std::vector<Point>& points,std::vector<double> &result_targetEdges_vector,std::vector<double> &result_targetRadius_vector) {
    // 计算边长 对于每个点 points1[i]，计算其与下一个点 points1[next_i] 之间的距离，targetEdges 中最后一个边长是由最后一个点 points1[points1.size() - 1] 和第一个点 points1[0] 构成的。
    ROS_INFO_STREAM("=============================计算目标多边形的边长 calculateTargetEdges 打印点坐标 , 点数量points.size()"<<points.size());
    try{
        for (size_t i = 0; i < points.size(); ++i) {
            size_t next_i = (i + 1) % points.size();
            //打印点坐标
            ROS_INFO_STREAM("-----------------------------------------------calculateTargetEdges--------------------------------------------------------------------------)");
            ROS_INFO_STREAM(" 打印当前坐标 Point " << i << ": (" << points[i].x << ", " << points[i].y << ")");
            ROS_INFO_STREAM(" 打印下一个坐标 Point " << next_i << ": (" << points[next_i].x << ", " << points[next_i].y << ")");
            double distance = calculateDistance(points[i], points[next_i]);
            if (    std::isnan(distance) || std::isinf(distance)) {
                std::cout << "计算目标多边形的边长 calculateTargetEdges edge is a valid isnan or finite number.不能加入边长计算。" << std::endl;
                return false;
            }
            double radius =    sqrt(pow(points[i].x, 2) + pow(points[i].y, 2));
            if (    std::isnan(radius) || std::isinf(radius)) {
                std::cout << "计算雷达扫描到的反光柱的半径 calculateTargetEdges edge is a valid isnan or finite number.不能加入边长计算。" << std::endl;
                return false;
            }
            result_targetRadius_vector.push_back(radius);
            result_targetEdges_vector.push_back(distance);
        }
    }catch (std::exception e) {
        ROS_INFO_STREAM("ERROR 计算目标多边形的边长 calculateTargetEdges error:"<<e.what());
        return false;
    }


    return true;
}


//--------------------------------------------------------------------------------

// 计算反光板组成的多边形的角度特征，目标多边形的角度
bool calculateTargetAngles(const std::vector<Point>& points,std::vector<double> &result_targetAngles_vector) {
    // 计算角度 targetAngles 中的第一个夹角，如果当前点是第一个点，则前一个点是最后一个点。（ 第一个夹角：  前一个点-当前点-最后一个点 ）
    ROS_INFO_STREAM("============================= 目标多边形的角度 calculateTargetAngles 打印点坐标 , 点数量points.size()"<<points.size());
    try{
        for (size_t i = 0; i < points.size(); ++i) {
            size_t prev_i = (i == 0) ? points.size() - 1 : i - 1;
            size_t next_i = (i + 1) % points.size();
            //打印点坐标
            ROS_INFO_STREAM("------------------------------------------calculateTargetAngles-------------------------------------------------------------------------------)");
            ROS_INFO_STREAM("打印当前坐标 Point " << i << ": (" << points[i].x << ", " << points[i].y << ")");
            ROS_INFO_STREAM(" 打印下一个坐标 Point " << next_i << ": (" << points[next_i].x << ", " << points[next_i].y << ")");
            double angle = calculateAngle(points[prev_i], points[i], points[next_i]);
            if (    std::isnan(angle) || std::isinf(angle)) {
                std::cout << "计算反光板组成的多边形的角度特征，目标多边形的角度 calculateTargetAngles angle is a valid isnan or finite number.不能加入角度计算。" << std::endl;
                return false;
            }
            result_targetAngles_vector.push_back(angle);
        }
    }catch (std::exception e) {
        std::cout << "ERROR 计算反光板组成的多边形的角度特征，目标多边形的角度 calculateTargetAngles error." << std::endl;
        return false;
    }

    return true;
}


//===============================================================================

// 辅助函数，用于检查距离值的有效性
bool areDistancesValid(const std::vector<double>& distances) {
    for (double d : distances) {
        if (d <= 0) return false;
    }
    return true;
}


//经典三边定位最小二乘法
Pose trilateration_with_yaw(const std::vector<Point>& anchors, const std::vector<double>& distances) {
    int n = anchors.size();

    // 输入数据验证
    if (n != distances.size() || !areDistancesValid(distances)) {
        // throw invalid_argument("Invalid input: Number of anchors must match number of distances and all distances must be positive.");
        std::cerr<<"Invalid input: Number of anchors must match number of distances and all distances must be positive.distances.size() " <<distances.size()<<std::endl;

        Pose pose;
        return pose;
    }

    MatrixXd A(n, 3);
    VectorXd b(n);

    for (int i = 0; i < n; ++i) {
        double x1 = anchors[i].x;
        double y1 = anchors[i].y;
        double d1 = distances[i];

        A(i, 0) = -2 * x1;
        A(i, 1) = -2 * y1;
        A(i, 2) = 1;

        b(i) = x1 * x1 + y1 * y1 - d1 * d1;
    }

    Vector3d result = A.colPivHouseholderQr().solve(b);
    Point point_result;
    point_result.x=result(0);
    point_result.y=result(1);


    /**
     // 检查解是否位于所有锚点构成的凸包内部，这是一个简化的合理性检查
     bool isValidSolution = true;
     for (int i = 0; i < n && isValidSolution; ++i) {
         std::cout<<"-----------------------------------------------"<<i <<std::endl;
         std::cout<<"-----------------------------------------anchors.size()------"<<anchors.size()<<std::endl;
         std::cout<<"----------------------------------------anchors[i].x-------"<<anchors[i].x <<std::endl;
         std::cout<<"-----------------------------------------anchors[i].y------"<<anchors[i].y <<std::endl;
         Point1 point1=anchors[i];
         if (distanceSquare1(point1, point_result) > distances[i] * distances[i]) {//todo 奇怪问题 distanceSquare1中获取不到 point1, point_result 的数据。 nan
             std::cout<<"error distances[i] * distances[i]: "<<distances[i] * distances[i]<<std::endl;
             std::cout<<"error distances[i]: "<< distances[i]<<std::endl;

             isValidSolution = false;
         }else{

             std::cout<<"distances[i] * distances[i]: "<<distances[i] * distances[i]<<std::endl;
             std::cout<<"distances[i]: "<< distances[i]<<std::endl;
         }
         std::cout<<"2----------------------------------------anchors[i].x-------"<<anchors[i].x <<std::endl;
         std::cout<<"2-----------------------------------------anchors[i].y------"<<anchors[i].y <<std::endl;

     }
     if (!isValidSolution) {
         throw runtime_error("The computed position does not seem to be consistent with the given distances.");
     }
     */

    // 改进偏航角计算，考虑使用第一个和第二个锚点的方向作为参考
    double refX = anchors[1].x - anchors[0].x;
    double refY = anchors[1].y - anchors[0].y;
    double yaw = atan2(refY, refX) - atan2(result(1), result(0)); // 注意：这里可能需要根据具体情况调整偏航角的计算逻辑

    // 确保偏航角范围在[-pi, pi] 6.28
    yaw = fmod(yaw + M_PI, 2 * M_PI) - M_PI;
    Pose pose;
    pose.position.x=result(0);
    pose.position.y=result(1);
    pose.position.yaw=yaw;
    pose.id=base_link_frame_id_;//TODO  计算得到的是  小车 base_link 在map下 的坐标
    return pose;
}

// 辅助函数，用于检查距离值的有效性
bool areDistancesValid( const std::vector<std::shared_ptr<ReflectorPanelPoint>> &reflectorPanelPoints) {
    for (auto ref : reflectorPanelPoints) {
        if (ref->base_linkTfanguangban_distances <= 0) return false;
    }
    return true;
}

// 辅助函数，用于检查距离值的有效性
bool areDistancesValid( const  std::vector<ReflectorPanelPointStruct> &structIndices_first ) {
    for (auto ref : structIndices_first) {
        if (ref.base_linkTfanguangban_distances <= 0) return false;
    }
    return true;
}



//开源反光板 /home/sukai/workspace/workspace_ros_car_noetic/src/VEnus/venus/mapping/carto_mapping/carto_mapping.cc
//直接法 (法三) 临时变量，目前 scan_CB_new 中用的 2024-08-19  7209
Pose optimizePoseWithTrilateration(const std::vector<ReflectorPanelPointStruct> &structIndices_first,Pose &robot_pose) {

    int n = structIndices_first.size();

    // 检查距离数据是否有效
    if (!areDistancesValid(structIndices_first)) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        Pose pose;
        return pose;
    }

    // todo 从tf::StampedTransform 提取初始位姿
//    double pose[3] = {
//            base_link_to_map_transform_.getOrigin().x(),
//            base_link_to_map_transform_.getOrigin().y(),
//            tf::getYaw(base_link_to_map_transform_.getRotation())
//    };
    //从 ComputeCurrentPose 提取初始位姿
    double pose[3] = {
            robot_pose.position.x,
            robot_pose.position.y,
            robot_pose.position.yaw
    };

    // 创建Ceres优化问题
    ceres::Problem problem;

    for (int i = 0; i < n; ++i) {
        auto ref = structIndices_first[i];
        double x1 = structIndices_first[i].x;
        double y1 = structIndices_first[i].y;
        double d3 = structIndices_first[i].base_linkTfanguangban_distances;
        double base_linkTfanguangban_to_map_x = structIndices_first[i].current_baselinkTscan_x;
        double base_linkTfanguangban_to_map_y = structIndices_first[i].current_baselinkTscan_y;


        // 为每个参考点添加残差块
//        problem.AddResidualBlock(
//                new ceres::AutoDiffCostFunction<TrilaterationResidual, 1, 3>(  //这表示创建了一个自动微分成本函数，它使用TrilaterationResidual类作为残差计算类，输出维度为1，输入参数维度为3。
//                        new TrilaterationResidual(x1, y1, d3)),
//                nullptr, pose);

//        problem.AddResidualBlock(
//                new ceres::AutoDiffCostFunction<TrilaterationResidual, 1, 3>(  //这表示创建了一个自动微分成本函数，它使用TrilaterationResidual类作为残差计算类，输出维度为1，输入参数维度为3。
//                        new TrilaterationResidual(x1, y1, d3)),
//                new ceres::CauchyLoss(0.2), pose);


        // 创建pair对，存储全局坐标和局部坐标
        pair<double, double> global_coords_fpt_pair_first(x1, y1);
        pair<double, double> local_coords_fpt_pair_second(base_linkTfanguangban_to_map_x, base_linkTfanguangban_to_map_y);

        problem.AddResidualBlock(new ceres::AutoDiffCostFunction<FeaturePairCost, 2, 3>(
                                         //new FeaturePairCost(fpt_pair.second, fpt_pair.first)),
                                         new FeaturePairCost(local_coords_fpt_pair_second, global_coords_fpt_pair_first)),
                                 new ceres::CauchyLoss(0.2), pose);

    }

    // 设置Ceres Solver选项
    ceres::Solver::Options options;
    // options.linear_solver_type = ceres::DENSE_QR;// 更改线性求解器
    //更改了线性求解器类型为 SPARSE_NORMAL_CHOLESKY，这通常比 DENSE_QR 更高效
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 更改线性求解器
    // options.linear_solver_type = ceres::DENSE_SCHUR; // 这对于小型和稀疏问题可能会有更好的表现。
    options.minimizer_progress_to_stdout = false;

    // 求解优化问题
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    // 获取优化后的位姿
    Pose mapTbaselink_pose;
    mapTbaselink_pose.position.x = pose[0];
    mapTbaselink_pose.position.y = pose[1];
    mapTbaselink_pose.position.yaw = pose[2];
    mapTbaselink_pose.id = map_frame_id_;

    return mapTbaselink_pose;
}
// 判断是否是原地旋转 previous_base_link_to_odom_transform_  base_link_to_odom_transform_
// 原地转到的 位移在0.0001-0.0003之间，角度大于0.01  , 行走时的位移在0.01-0.02之间，角度大于等于0.0001
bool CheckInPlaceRotation(
        const tf::StampedTransform& predicted_odomTbase_transform,
        const tf::StampedTransform& previous_odomTbase_transform,
        double angle_threshold = 0.01,  // 角度阈值（弧度）
        double distance_threshold = 0.002) {  // 位移阈值（米）

    // 获取当前帧的位置
    tf::Vector3 current_origin = predicted_odomTbase_transform.getOrigin();
    //上一帧的位姿的位置
    tf::Vector3 previous_origin = previous_odomTbase_transform.getOrigin();
    // 获取当前帧的位姿
    tf::Quaternion current_rotation = predicted_odomTbase_transform.getRotation();
    //上一帧的位姿
    tf::Quaternion previous_rotation = previous_odomTbase_transform.getRotation();

    // 计算位移
    double delta_x = current_origin.x() - previous_origin.x();
    double delta_y = current_origin.y() - previous_origin.y();
    double distance_moved = std::sqrt(delta_x * delta_x + delta_y * delta_y);
    distance_moved=std::fabs(distance_moved);
    // 计算旋转角度差
    double delta_yaw = tf::getYaw(current_rotation) - tf::getYaw(previous_rotation);
    // 保证角度在 [-π, π] 之间
    delta_yaw = std::fmod(delta_yaw + M_PI, 2 * M_PI) - M_PI;
    delta_yaw=std::fabs(delta_yaw);

    if(time_log_fig_==12){//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。

        std::string resultType = "CheckInPlaceRotation";
        std::string resultChineseMessage = " 使用上一次 上一帧的位姿odomTbaselink与当前帧的位姿odomTbaselink之间的差异  判断移动机器人是否是在原地旋转";
        std::string resultENMessage = "Determine whether the mobile robot is rotating in place by using the difference between the previous frame's pose odomTbaselink and the current frame's pose odomTbaselink";
        std::string className = __FILE__; // 更改为实际的文件名
        int classLine = __LINE__; // 更改为实际的行号
        double duration=0.0;

        // 检测是否原地旋转
        if (distance_moved <= distance_threshold && delta_yaw >= angle_threshold) {
            //   原地旋转
            resultChineseMessage="正在原地旋转， 计算旋转角度差： "+ std::to_string(delta_yaw)+" ，距离差： "+ std::to_string(distance_moved)+" ,距离阀值： "+ std::to_string(distance_threshold)+" ,角度阀值： "+ std::to_string(angle_threshold);
        } else {
            resultChineseMessage="非原地旋转，计算旋转角度差: "+ std::to_string(delta_yaw)+" ，距离差： "+ std::to_string(distance_moved)+" ,距离阀值： "+ std::to_string(distance_threshold)+" ,角度阀值： "+ std::to_string(angle_threshold);
        }

        ///trilateration_time_log
        publishTrilaterationLog( duration, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_
                ,previous_odomTbase_transform,predicted_odomTbase_transform
                ,delta_x,delta_y,delta_yaw,distance_moved,distance_threshold,angle_threshold);


    }


    // 检测是否原地旋转
    if (distance_moved <= distance_threshold && delta_yaw >= angle_threshold) {
        return true;//正在原地旋转
    } else {
        return false;
    }
}
//----------------------------------------------------------------------------------------
// 判断是否是原地旋转 previous_base_link_to_odom_transform_  base_link_to_odom_transform_
// 原地转到的 位移在0.0001-0.0003之间，角度大于0.01  , 行走时的位移在0.01-0.02之间，角度大于等于0.0001
bool CheckInPlaceRotation(
        const tf::Transform& predicted_odomTbase_transform,
        const tf::Transform& previous_odomTbase_transform,
        double angle_threshold = 0.01,  // 角度阈值（弧度）
        double distance_threshold = 0.002) {  // 位移阈值（米）

    // 获取当前帧的位置
    tf::Vector3 current_origin = predicted_odomTbase_transform.getOrigin();
    //上一帧的位姿的位置
    tf::Vector3 previous_origin = previous_odomTbase_transform.getOrigin();
    // 获取当前帧的位姿
    tf::Quaternion current_rotation = predicted_odomTbase_transform.getRotation();
    //上一帧的位姿
    tf::Quaternion previous_rotation = previous_odomTbase_transform.getRotation();

    // 计算位移
    double delta_x = current_origin.x() - previous_origin.x();
    double delta_y = current_origin.y() - previous_origin.y();
    double distance_moved = std::sqrt(delta_x * delta_x + delta_y * delta_y);
    distance_moved=std::fabs(distance_moved);
    // 计算旋转角度差
    double delta_yaw = tf::getYaw(current_rotation) - tf::getYaw(previous_rotation);
    // 保证角度在 [-π, π] 之间
    delta_yaw = std::fmod(delta_yaw + M_PI, 2 * M_PI) - M_PI;
    delta_yaw=std::fabs(delta_yaw);

    if(time_log_fig_==12){//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。

        std::string resultType = "CheckInPlaceRotation";
        std::string resultChineseMessage = " 使用上一次 上一帧的位姿odomTbaselink与当前帧的位姿odomTbaselink之间的差异  判断移动机器人是否是在原地旋转";
        std::string resultENMessage = "Determine whether the mobile robot is rotating in place by using the difference between the previous frame's pose odomTbaselink and the current frame's pose odomTbaselink";
        std::string className = __FILE__; // 更改为实际的文件名
        int classLine = __LINE__; // 更改为实际的行号
        double duration=0.0;

        // 检测是否原地旋转
        if (distance_moved <= distance_threshold && delta_yaw >= angle_threshold) {
            //   原地旋转
            resultChineseMessage="正在原地旋转， 计算旋转角度差： "+ std::to_string(delta_yaw)+" ，距离差： "+ std::to_string(distance_moved)+" ,距离阀值： "+ std::to_string(distance_threshold)+" ,角度阀值： "+ std::to_string(angle_threshold);
        } else {
            resultChineseMessage="非原地旋转，计算旋转角度差: "+ std::to_string(delta_yaw)+" ，距离差： "+ std::to_string(distance_moved)+" ,距离阀值： "+ std::to_string(distance_threshold)+" ,角度阀值： "+ std::to_string(angle_threshold);
        }

        ///trilateration_time_log
        publishTrilaterationLog( duration, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_
                ,previous_odomTbase_transform,predicted_odomTbase_transform
                ,delta_x,delta_y,delta_yaw,distance_moved,distance_threshold,angle_threshold);


    }


    // 检测是否原地旋转
    if (distance_moved <= distance_threshold && delta_yaw >= angle_threshold) {
        return true;
    } else {
        return false;
    }
}
//================================== 直接法 ============================================================

//开源反光板 /home/sukai/workspace/workspace_ros_car_noetic/src/VEnus/venus/mapping/carto_mapping/carto_mapping.cc
//直接法 (法三) 临时变量，目前 scan_CB_new 中用的 2024-08-19  7209，  robot_pose =  base_link_to_map_transform_
Pose optimizePoseWithTrilateration_new(const std::vector<ReflectorPanelPointStruct> &structIndices_first,Pose &robot_pose) {

    int n = structIndices_first.size();

    // 检查距离数据是否有效
    if (!areDistancesValid(structIndices_first)) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        Pose pose;
        return pose;
    }

    // todo 从tf::StampedTransform 提取初始位姿
//    double pose[3] = {
//            base_link_to_map_transform_.getOrigin().x(),
//            base_link_to_map_transform_.getOrigin().y(),
//            tf::getYaw(base_link_to_map_transform_.getRotation())
//    };
    //从 ComputeCurrentPose 提取初始位姿，当前的 base_link_to_map_transform_ 位置数据
    double pose[3] = {
            robot_pose.position.x,
            robot_pose.position.y,
            robot_pose.position.yaw
    };

    // 初始化上一帧的位姿变换
    tf::StampedTransform previous_transform = previous_base_link_to_odom_transform_; // 假设你已经保存了上一帧的数据 time_log_fig_==12
    //tf::StampedTransform previous_transform = previous_base_link_to_map_transform_; // 假设你已经保存了上一帧的数据

    // 判断是否是原地旋转 , 原地转到的 位移在0.0001-0.0003之间，角度大于0.01  , 行走时的位移在0.01-0.02之间，角度大于等于0.0001  .previous_base_link_to_odom_transform_  base_link_to_odom_transform_
    //  bool is_in_place_rotation = CheckInPlaceRotation(base_link_to_odom_transform_, previous_transform,,angle_threshold_,distance_threshold_);
    bool is_in_place_rotation =false;// 暂时不在ComplexResidual这里作不优化
    // 创建Ceres优化问题
    ceres::Problem problem;

    for (int i = 0; i < n; ++i) {
        auto ref = structIndices_first[i];
        double x1 = structIndices_first[i].x;
        double y1 = structIndices_first[i].y;
        double d3 = structIndices_first[i].base_linkTfanguangban_distances;
        double base_linkTfanguangban_to_map_x = structIndices_first[i].current_baselinkTscan_x;
        double base_linkTfanguangban_to_map_y = structIndices_first[i].current_baselinkTscan_y;


        // 为每个参考点添加残差块
//        problem.AddResidualBlock(
//                new ceres::AutoDiffCostFunction<TrilaterationResidual, 1, 3>(  //这表示创建了一个自动微分成本函数，它使用TrilaterationResidual类作为残差计算类，输出维度为1，输入参数维度为3。
//                        new TrilaterationResidual(x1, y1, d3)),
//                nullptr, pose);

//        problem.AddResidualBlock(
//                new ceres::AutoDiffCostFunction<TrilaterationResidual, 1, 3>(  //这表示创建了一个自动微分成本函数，它使用TrilaterationResidual类作为残差计算类，输出维度为1，输入参数维度为3。
//                        new TrilaterationResidual(x1, y1, d3)),
//                new ceres::CauchyLoss(0.2), pose);


        // 创建pair对，存储全局坐标和局部坐标
        pair<double, double> global_coords_fpt_pair_first(x1, y1);//存储全局坐
        pair<double, double> local_coords_fpt_pair_second(base_linkTfanguangban_to_map_x, base_linkTfanguangban_to_map_y);
        //第二个元素的数据为了计算当前移动机器人的姿态
        pair<double, double> global_coords_fpt_pair_first2;
        pair<double, double> local_coords_fpt_pair_second2;
        if(structIndices_first.size()>=1 && i ==0){
            global_coords_fpt_pair_first2=std::make_pair(structIndices_first[1].x,  structIndices_first[1].y);
            local_coords_fpt_pair_second2=std::make_pair(structIndices_first[1].current_baselinkTscan_x, structIndices_first[1].current_baselinkTscan_y);

        }else if( i !=1 && i !=0){
            global_coords_fpt_pair_first2=std::make_pair(structIndices_first[0].x,  structIndices_first[0].y);
            local_coords_fpt_pair_second2=std::make_pair(structIndices_first[0].current_baselinkTscan_x, structIndices_first[0].current_baselinkTscan_y);

        }

//https://blog.csdn.net/lovely_yoshino/article/details/136453573?spm=1001.2014.3001.5506
//        problem.AddResidualBlock(new ceres::AutoDiffCostFunction<FeaturePairCost, 2, 3>(
//                                         //new FeaturePairCost(fpt_pair.second, fpt_pair.first)),
//                                         new FeaturePairCost(local_coords_fpt_pair_second, global_coords_fpt_pair_first)),
//                                 new ceres::CauchyLoss(0.2), pose);


        // 添加残差块 4：代表 residual 数组的长度，即我们现在有4个残差项。3：表示 pose 数组的维度（x, y, yaw）。
        problem.AddResidualBlock(
                new ceres::AutoDiffCostFunction<ComplexResidual, 4, 3>(
                        new ComplexResidual(
                                local_coords_fpt_pair_second, local_coords_fpt_pair_second2,
                                global_coords_fpt_pair_first, global_coords_fpt_pair_first2,

                                d3, previous_transform, is_in_place_rotation
                        )
                ),
                new ceres::CauchyLoss(0.2), pose
        );


//解释：
//
//    6：代表 residual 数组的长度，即我们现在有6个残差项。
//    3：表示 pose 数组的维度（x, y, yaw）。
//       通过这种方式，你可以自由地添加新的残差项，并在 Ceres 优化中施加更多的约束或目标。确保新增的残差项有明确的物理意义，并与优化目标一致。
//        problem.AddResidualBlock(
//                new ceres::AutoDiffCostFunction<ComplexResidual, 6, 3>(
//                        new ComplexResidual(
//                                global_coords_fpt_pair_first, global_coords_fpt_pair_first2,
//                                local_coords_fpt_pair_second, local_coords_fpt_pair_second2,
//                                d3, previous_transform, is_in_place_rotation
//                        )
//                ),
//                new ceres::CauchyLoss(0.2), pose
//        );





    }

    // 设置Ceres Solver选项
    ceres::Solver::Options options;
    // options.linear_solver_type = ceres::DENSE_QR;// 更改线性求解器
    //更改了线性求解器类型为 SPARSE_NORMAL_CHOLESKY，这通常比 DENSE_QR 更高效
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 更改线性求解器
    //options.linear_solver_type = ceres::DENSE_SCHUR; // 这对于小型和稀疏问题可能会有更好的表现。
    options.minimizer_progress_to_stdout = false;
    // options.minimizer_progress_to_stdout = true; // 输出优化进展信息
    // options.max_num_iterations = 100; // 设置最大迭代次数
    // 求解优化问题
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    // 获取优化后的位姿
    Pose mapTbaselink_pose;
    mapTbaselink_pose.position.x = pose[0];
    mapTbaselink_pose.position.y = pose[1];
    mapTbaselink_pose.position.yaw = pose[2];
    mapTbaselink_pose.id = map_frame_id_;

    // 计算并广播transform（可选）
    tf::Transform mapTbaselink_transform;
    mapTbaselink_transform.setOrigin(tf::Vector3(mapTbaselink_pose.position.x, mapTbaselink_pose.position.y, 0.0));
    tf::Quaternion mapTbaselink_q;
    mapTbaselink_q.setRPY(0, 0, mapTbaselink_pose.position.yaw);
    mapTbaselink_transform.setRotation(mapTbaselink_q);

    if( test_num_==42)
        tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbaselink_transform, ros::Time::now(), map_frame_id_, "optimizePoseWithTrilateration_new_mapTbaselink_transform_transform_trilateration_with_yaw_new"));


    return mapTbaselink_pose;
}





//开源反光板 /home/sukai/workspace/workspace_ros_car_noetic/src/VEnus/venus/mapping/carto_mapping/carto_mapping.cc
//直接法 (法三) 临时变量，目前 scan_CB_new 中用的 2024-08-19  7209，  robot_pose =  base_link_to_map_transform_

//与 optimizePoseWithTrilateration_new 对比，增加了通过里程计数据动态计算反光柱与baseLink的距离
Pose optimizePoseWithTrilateration_new1(const std::vector<ReflectorPanelPointStruct> &structIndices_first,Pose &robot_pose) {

    int n = structIndices_first.size();

    // 检查距离数据是否有效
    if (!areDistancesValid(structIndices_first)) {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        Pose pose;
        return pose;
    }


    tf::StampedTransform odomTlaser_now_transform;
    try {
        //todo  odomTlaser
        tf_listener_new_->waitForTransform(odom_frame_id_, laser_frame_id_,
                                           ros::Time(0), ros::Duration(1.0));
        tf_listener_new_->lookupTransform(odom_frame_id_, laser_frame_id_, ros::Time(0), odomTlaser_now_transform);

    }catch (tf::TransformException& ex) {
        ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());
        Pose pose;
        return pose;
    }




    // todo 从tf::StampedTransform 提取初始位姿
//    double pose[3] = {
//            base_link_to_map_transform_.getOrigin().x(),
//            base_link_to_map_transform_.getOrigin().y(),
//            tf::getYaw(base_link_to_map_transform_.getRotation())
//    };
    //从 ComputeCurrentPose 提取初始位姿，当前的 base_link_to_map_transform_ 位置数据
    double pose[3] = {
            robot_pose.position.x,
            robot_pose.position.y,
            robot_pose.position.yaw
    };

    //-------------------------------
    //-----------------------------------------------
    // 初始化上一帧的位姿变换 odomTbase_link_transform_1
    //tf::StampedTransform previous_transform = previous_base_link_to_odom_transform_; // 假设你已经保存了上一帧的数据 time_log_fig_==12
    //tf::StampedTransform previous_transform = odomTbase_link_transform_1;
    //tf::StampedTransform previous_transform = previous_base_link_to_map_transform_; // 假设你已经保存了上一帧的数据

    // 判断是否是原地旋转 , 原地转到的 位移在0.0001-0.0003之间，角度大于0.01  , 行走时的位移在0.01-0.02之间，角度大于等于0.0001  .previous_base_link_to_odom_transform_  base_link_to_odom_transform_
    //  bool is_in_place_rotation = CheckInPlaceRotation(base_link_to_odom_transform_, previous_transform,,angle_threshold_,distance_threshold_);
    bool is_in_place_rotation =false;// 暂时不在ComplexResidual这里作不优化
    // 创建Ceres优化问题
    ceres::Problem problem;


    double previous_x1 = 0.0;
    double previous_y1 = 0.0;

    double previous_base_linkTfanguangban_to_map_x = 0.0;
    double previous_base_linkTfanguangban_to_map_y = 0.0;

    for (int i = 0; i < n; ++i) {
        auto ref = structIndices_first[i];
        //部署在地图中的反光柱全局坐标
        double x1 = ref.x;
        double y1 = ref.y;


        /**
         * 有用 2024-09-14
        //----------------------------------------
        //todo ---------------------动态调整 雷达扫描到的 反光柱的位置 ---------------------------
        //todo 1. 上一次 通过雷达探测到的实时 odom坐标系下的反光柱坐标 2024-09-14
        tf::Transform previous_odomTscanpoint_transform;
        previous_odomTscanpoint_transform.setOrigin(tf::Vector3(ref.circle.odomTscan_x, ref.circle.odomTscan_y, ref.circle.odomTscan_z ));
        //tf::Quaternion q;
        tf::Quaternion previous_odomTscanpoint_q(ref.circle.odomTscan_rx, ref.circle.odomTscan_ry, ref.circle.odomTscan_rz, ref.circle.odomTscan_rw); //  x, y, z，w, 分量
        // q.setRPY(0, 0, reflectorPanelPoint->laserTscan_yaw);
        previous_odomTscanpoint_transform.setRotation(previous_odomTscanpoint_q);//base_link_to_odom_transform;//odomTbase_link

        tf::Transform  laserTscanPoint_now = odomTlaser_now_transform.inverse() * previous_odomTscanpoint_transform;

        tf::Transform  base_linkTfanguangban_transform  = laser_to_base_link_transform_ * laserTscanPoint_now;


        //base_linkTfanguangban
        double base_linkTfanguangban_to_map_x=  base_linkTfanguangban_transform.getOrigin().getX();
        double base_linkTfanguangban_to_map_y=  base_linkTfanguangban_transform.getOrigin().getY();
        double base_linkTfanguangban_to_map_z=  base_linkTfanguangban_transform.getOrigin().getZ();

        double d3 = std::sqrt(std::pow(base_linkTfanguangban_to_map_x , 2) + std::pow(base_linkTfanguangban_to_map_y, 2) );

        // 初始化上一帧的位姿变换 odomTbase_link_transform_1
        tf::StampedTransform previous_transform = ref.circle.odomTbase_link_transform_1;
        */


        double base_linkTfanguangban_to_map_x= ref.current_baselinkTscan_x;
        double base_linkTfanguangban_to_map_y= ref.current_baselinkTscan_y;
        double base_linkTfanguangban_to_map_z= ref.current_baselinkTscan_z;

        double d3 = std::sqrt(std::pow(base_linkTfanguangban_to_map_x , 2) + std::pow(base_linkTfanguangban_to_map_y, 2) );


        tf::StampedTransform previous_transform =ref.circle.odomTbase_link_transform_1;
        //----------------------------------------------------------------------------

        // 为每个参考点添加残差块
//        problem.AddResidualBlock(
//                new ceres::AutoDiffCostFunction<TrilaterationResidual, 1, 3>(  //这表示创建了一个自动微分成本函数，它使用TrilaterationResidual类作为残差计算类，输出维度为1，输入参数维度为3。
//                        new TrilaterationResidual(x1, y1, d3)),
//                nullptr, pose);

//        problem.AddResidualBlock(
//                new ceres::AutoDiffCostFunction<TrilaterationResidual, 1, 3>(  //这表示创建了一个自动微分成本函数，它使用TrilaterationResidual类作为残差计算类，输出维度为1，输入参数维度为3。
//                        new TrilaterationResidual(x1, y1, d3)),
//                new ceres::CauchyLoss(0.2), pose);


        // 创建pair对，存储全局坐标和局部坐标
        pair<double, double> global_coords_fpt_pair_first(x1, y1);////部署在地图中的反光柱全局坐标
        /// 雷达探测到的反光柱在baselink下的坐标
        pair<double, double> local_coords_fpt_pair_second(base_linkTfanguangban_to_map_x, base_linkTfanguangban_to_map_y);

        //第二个元素的数据为了计算当前移动机器人的姿态
        pair<double, double> global_coords_fpt_pair_first2;
        pair<double, double> local_coords_fpt_pair_second2;


        if(structIndices_first.size()>1 && i ==0){
            global_coords_fpt_pair_first2=std::make_pair(structIndices_first[1].x,  structIndices_first[1].y);
            local_coords_fpt_pair_second2=std::make_pair(structIndices_first[1].current_baselinkTscan_x, structIndices_first[1].current_baselinkTscan_y);

        }else if( i >0){
//            global_coords_fpt_pair_first2=std::make_pair(structIndices_first[0].x,  structIndices_first[0].y);
//            local_coords_fpt_pair_second2=std::make_pair(structIndices_first[0].current_baselinkTscan_x, structIndices_first[0].current_baselinkTscan_y);

            global_coords_fpt_pair_first2=std::make_pair(previous_x1,  previous_y1);
            local_coords_fpt_pair_second2=std::make_pair(previous_base_linkTfanguangban_to_map_x,previous_base_linkTfanguangban_to_map_y);

        }


//        global_coords_fpt_pair_first2=std::make_pair(x1,  y1);
//        local_coords_fpt_pair_second2=std::make_pair(base_linkTfanguangban_to_map_x, base_linkTfanguangban_to_map_y);



//https://blog.csdn.net/lovely_yoshino/article/details/136453573?spm=1001.2014.3001.5506
//        problem.AddResidualBlock(new ceres::AutoDiffCostFunction<FeaturePairCost, 2, 3>(
//                                         //new FeaturePairCost(fpt_pair.second, fpt_pair.first)),
//                                         new FeaturePairCost(local_coords_fpt_pair_second, global_coords_fpt_pair_first)),
//                                 new ceres::CauchyLoss(0.2), pose);


        // 添加残差块 4：代表 residual 数组的长度，即我们现在有4个残差项。3：表示 pose 数组的维度（x, y, yaw）。
        problem.AddResidualBlock(
                new ceres::AutoDiffCostFunction<ComplexResidual, 4, 3>(
                        new ComplexResidual(
                                local_coords_fpt_pair_second, local_coords_fpt_pair_second2,
                                global_coords_fpt_pair_first, global_coords_fpt_pair_first2,

                                d3, previous_transform, is_in_place_rotation
                        )
                ),
                new ceres::CauchyLoss(0.2), pose
        );

        //保存上一次数据
        previous_x1 = x1;
        previous_y1 = y1;
        previous_base_linkTfanguangban_to_map_x = base_linkTfanguangban_to_map_x;
        previous_base_linkTfanguangban_to_map_y = base_linkTfanguangban_to_map_y;

//解释：
//
//    6：代表 residual 数组的长度，即我们现在有6个残差项。
//    3：表示 pose 数组的维度（x, y, yaw）。
//       通过这种方式，你可以自由地添加新的残差项，并在 Ceres 优化中施加更多的约束或目标。确保新增的残差项有明确的物理意义，并与优化目标一致。
//        problem.AddResidualBlock(
//                new ceres::AutoDiffCostFunction<ComplexResidual, 6, 3>(
//                        new ComplexResidual(
//                                global_coords_fpt_pair_first, global_coords_fpt_pair_first2,
//                                local_coords_fpt_pair_second, local_coords_fpt_pair_second2,
//                                d3, previous_transform, is_in_place_rotation
//                        )
//                ),
//                new ceres::CauchyLoss(0.2), pose
//        );





    }

    // 设置Ceres Solver选项
    ceres::Solver::Options options;
    // options.linear_solver_type = ceres::DENSE_QR;// 更改线性求解器
    //更改了线性求解器类型为 SPARSE_NORMAL_CHOLESKY，这通常比 DENSE_QR 更高效
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 更改线性求解器
    //options.linear_solver_type = ceres::DENSE_SCHUR; // 这对于小型和稀疏问题可能会有更好的表现。
    options.minimizer_progress_to_stdout = false;
    // options.minimizer_progress_to_stdout = true; // 输出优化进展信息
    // options.max_num_iterations = 100; // 设置最大迭代次数
    // 求解优化问题
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    // 获取优化后的位姿
    Pose mapTbaselink_pose;
    mapTbaselink_pose.position.x = pose[0];
    mapTbaselink_pose.position.y = pose[1];
    mapTbaselink_pose.position.yaw = pose[2];
    mapTbaselink_pose.id = map_frame_id_;

    // 计算并广播transform（可选）
    tf::Transform mapTbaselink_transform;
    mapTbaselink_transform.setOrigin(tf::Vector3(mapTbaselink_pose.position.x, mapTbaselink_pose.position.y, 0.0));
    tf::Quaternion mapTbaselink_q;
    mapTbaselink_q.setRPY(0, 0, mapTbaselink_pose.position.yaw);
    mapTbaselink_transform.setRotation(mapTbaselink_q);


    if( test_num_==42)
        tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbaselink_transform, ros::Time::now(), map_frame_id_, "optimizePoseWithTrilateration_new_mapTbaselink_transform_transform_trilateration_with_yaw_new"));


    return mapTbaselink_pose;
}

//==========================================经典三边定位最小二乘法=====================================================================
//经典三边定位最小二乘法 (法一) 临时变量，目前 scan_CB_new 中用的 2024-08-19  7209
//与 trilateration_with_yaw_new 对比，增加了通过里程计数据动态计算反光柱与baseLink的距离
Pose trilateration_with_yaw_new1(const std::vector<ReflectorPanelPointStruct> &structIndices_first) {

//    （法一）
//    参考： /home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/image/baselink姿态计算.png
//    已知两个反光柱a,b分别在map坐标系的坐标A，B及在baselink中的坐标a,b。[不使用2个反光柱，用一个反光柱和一个 baselink 计算姿态也可以的]
//    ->                       ->
//    分别计算 AB 向量在map中的姿态YAW，与 ab 向量在 baselink 中的姿态yaw。
//    baselink在map中的姿态Yaw=YAW-yaw。


    //  std::this_thread::sleep_for(std::chrono::milliseconds(10));
    int n = structIndices_first.size();
    // 输入数据验证， 检查距离数据是否有效
    if (!areDistancesValid(structIndices_first)) {
        // throw invalid_argument("Invalid input: Number of anchors must match number of distances and all distances must be positive.");
        std::cerr<<"4411 经典三边定位最小二乘法 (法一) 临时变量 trilateration_with_yaw_new Invalid input: Number of anchors must match number of distances and all distances must be positive." <<std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        Pose pose;
        return pose;
    }

    tf::StampedTransform odomTlaser_now_transform;
    try {
        //todo  odomTlaser
        tf_listener_new_->waitForTransform(odom_frame_id_, laser_frame_id_,
                                           ros::Time(0), ros::Duration(1.0));
        tf_listener_new_->lookupTransform(odom_frame_id_, laser_frame_id_, ros::Time(0), odomTlaser_now_transform);

    }catch (tf::TransformException& ex) {
        ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());
        Pose pose;
        return pose;
    }


    //初始化，定义一个n行3列的矩阵A，用于存储系数。
    MatrixXd A(n, 3);
    //VectorXd b(n); 定义一个n维的列向量b，用于存储常数项。
    VectorXd b(n);
    //数据准备，通过循环遍历reflectorPanelPoint
    for (int i = 0; i < n; ++i) {
        // std::this_thread::sleep_for(std::chrono::milliseconds(10));
        //这是一个包含ReflectorPanelPoint类型的指针数组或容器，每个元素指向一个reflectorPanelPoints类
        auto ref =  structIndices_first[i];
        //反射点的在map下的坐标(x1, y1)
        double x1 = structIndices_first[i].x;
        double y1 = structIndices_first[i].y;
        double z1 = structIndices_first[i].z;
        //反光板到 base_link 的距离 [基于雷达探测到的数据] 【定位丢失时这个数据是可靠的（未涉及到了map坐标系， 是基于 lase - > baselink 计算的）】
        double d3 = structIndices_first[i].base_linkTfanguangban_distances;//【法二计算姿态】

        //================================================================================
        /**

       //todo ---------------------动态调整 雷达扫描到的 反光柱的位置 ---------------------------
       //todo 1. 上一次 通过雷达探测到的实时 odom坐标系下的反光柱坐标 2024-09-14
       tf::Transform previous_odomTscanpoint_transform;
       previous_odomTscanpoint_transform.setOrigin(tf::Vector3(ref.circle.odomTscan_x, ref.circle.odomTscan_y, ref.circle.odomTscan_z ));
       //tf::Quaternion q;
       tf::Quaternion previous_odomTscanpoint_q(ref.circle.odomTscan_rx, ref.circle.odomTscan_ry, ref.circle.odomTscan_rz, ref.circle.odomTscan_rw); //  x, y, z，w, 分量
       // q.setRPY(0, 0, reflectorPanelPoint->laserTscan_yaw);
       previous_odomTscanpoint_transform.setRotation(previous_odomTscanpoint_q);//base_link_to_odom_transform;//odomTbase_link

       tf::Transform  laserTscanPoint_now = odomTlaser_now_transform.inverse() * previous_odomTscanpoint_transform;

       tf::Transform  base_linkTfanguangban_transform  = laser_to_base_link_transform_ * laserTscanPoint_now;


       //base_linkTfanguangban
       double base_linkTfanguangban_to_map_x=  base_linkTfanguangban_transform.getOrigin().getX();
       double base_linkTfanguangban_to_map_y=  base_linkTfanguangban_transform.getOrigin().getY();
       double base_linkTfanguangban_to_map_z=  base_linkTfanguangban_transform.getOrigin().getZ();

       d3 = std::sqrt(std::pow(base_linkTfanguangban_to_map_x , 2) + std::pow(base_linkTfanguangban_to_map_y, 2) );

       // 初始化上一帧的位姿变换 odomTbase_link_transform_1
       tf::StampedTransform previous_transform = ref.circle.odomTbase_link_transform_1;
       */


//        double base_linkTfanguangban_to_map_x= ref.current_baselinkTscan_x;
//        double base_linkTfanguangban_to_map_y= ref.current_baselinkTscan_y;
//        double base_linkTfanguangban_to_map_z= ref.current_baselinkTscan_z;
//
//        double d3 = std::sqrt(std::pow(base_linkTfanguangban_to_map_x , 2) + std::pow(base_linkTfanguangban_to_map_y, 2) );
//
//
//        tf::StampedTransform previous_transform =ref.circle.odomTbase_link_transform_1;

        //----------------------------------------------------------------------------


        //        std::cerr<<"3781 经典三边定位最小二乘法 (法一):trilateration_with_yaw_new,id: " <<reflectorPanelPoints[i]->id<<std::endl;
//        std::cerr<<"x1: " <<x1<<std::endl;
//        std::cerr<<"y1: " <<y1<<std::endl;
//        std::cerr<<"base_linkTfanguangban_distances: " <<d3<<std::endl;

        //构建线性系统，计算矩阵A的第i行，这形成了一个关于未知点坐标(x, y)的线性方程组，其中每个方程来源于一个反射点到估计点距离的平方与实际测量距离平方之差
        A(i, 0) = -2 * x1;
        A(i, 1) = -2 * y1;
        A(i, 2) = 1;
        //构建线性系统，计算向量b的第i个元素，即每个反射点坐标平方和减去测量距离平方。
        b(i) = x1 * x1 + y1 * y1 - d3 * d3;//得到 baselink的坐标

    }

    //解线性系统，使用列主元Householder QR分解方法求解线性方程组Ax=c。
    Vector3d mapTbaselink_result = A.colPivHouseholderQr().solve(b);//baselink 的坐标
//----------------------------------------------------------------------

    Pose mapTbaselink_pose;//baselink的坐标
    //todo sukai  2024-07-12 取反，发现数据符号相反；
    mapTbaselink_pose.position.x=-mapTbaselink_result(0);//取反
    mapTbaselink_pose.position.y=-mapTbaselink_result(1);//取反
    mapTbaselink_pose.id=map_frame_id_;//TODO  计算得到的是  小车 base_link 在map下 的坐标



//    std::cerr<<"mapTbaselink_pose.position.x: " <<mapTbaselink_pose.position.x<<std::endl;
//    std::cerr<<"mapTbaselink_pose.position.y: " <<mapTbaselink_pose.position.y<<std::endl;
//
//
//    std::cerr<<"---------------------------------------------------------" <<std::endl;
//    std::cerr<<"reflectorPanelPoints[0]->x: " <<reflectorPanelPoints[0]->x<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->x: " <<reflectorPanelPoints[1]->x<<std::endl;
//
//    std::cerr<<"reflectorPanelPoints[0]->y: " <<reflectorPanelPoints[0]->y<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->y: " <<reflectorPanelPoints[1]->y<<std::endl;
//    std::cerr<<"---------------------------------------------------------" <<std::endl;
//    std::cerr<<"reflectorPanelPoints[0]->current_baselinkTscan_x: " <<reflectorPanelPoints[0]->current_baselinkTscan_x<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->current_baselinkTscan_x: " <<reflectorPanelPoints[1]->current_baselinkTscan_x<<std::endl;
//
//    std::cerr<<"reflectorPanelPoints[0]->current_baselinkTscan_y: " <<reflectorPanelPoints[0]->current_baselinkTscan_y<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->current_baselinkTscan_y: " <<reflectorPanelPoints[1]->current_baselinkTscan_y<<std::endl;


    //以下这段代码假设 result(0)和result(1) 是估计点的x和y坐标，且之前已经正确解决了线性系统。此外，计算的偏航角差值可能需要根据实际情况进行调整，例如考虑坐标系统的定义和旋转的方向。
    // 改进偏航角计算，考虑使用第一个和第二个锚点的方向作为参考
    //这里计算了从第一个反射点到第二个反射点的向量（refX和refY），这代表了参考方向。
    double refX = structIndices_first[1].x - structIndices_first[0].x;//yaw2 -map
    double refY = structIndices_first[1].y - structIndices_first[0].y;//yaw2 -map

    //反光柱的在laser下的坐标(laserTscan_x, laserTscan_y)
//    double scan_x1 = reflectorPanelPoints[1]->laserTscan_x - reflectorPanelPoints[0]->laserTscan_x;//yaw1 -baselink
//    double scan_y1 = reflectorPanelPoints[1]->laserTscan_y - reflectorPanelPoints[0]->laserTscan_y;//yaw1 -baselink
    //反光柱的在baselink下的坐标(current_baselinkTscan_x, current_baselinkTscan_y)
    double scan_x1 = structIndices_first[1].current_baselinkTscan_x - structIndices_first[0].current_baselinkTscan_x;//yaw1 -baselink
    double scan_y1 = structIndices_first[1].current_baselinkTscan_y - structIndices_first[0].current_baselinkTscan_y;//yaw1 -baselink

    //计算参考方向的偏航角: yaw_ref=atan2(refY, refX); 计算估计点的偏航角:yaw_est = atan2(result(1), result(0));。atan2函数返回的范围是[-π, π]
    //  double yaw =  atan2(scan_x1, scan_y1)-atan2(refY, refX);
    double yaw2  = atan2(refY, refX); // 向量在 map 坐标系中的姿态
    double yaw1  = atan2(scan_y1, scan_x1);// 向量在 baselink 坐标系中的姿态
    double yaw =  yaw2-yaw1;//小车baselink的姿态=向量在 map 坐标系中的姿态-向量在 baselink 坐标系中的姿态 【 yaw = yaw2 - yaw1 】
    //计算偏航角差值: yaw = yaw_ref - yaw_est; 这里计算了参考方向的偏航角与估计点的偏航角之间的差值；                  atan2函数返回的范围是[-π, π] ，确保偏航角范围在[-pi, pi]
    //这段代码确保yaw的值在[-π, π]范围内。fmod函数用于取模运算，这里的目的是将偏航角差值映射回标准的2π周期内。首先加上π，然后对2π取模，最后减去π，确保结果在[-π, π]之间。
    mapTbaselink_pose.position.yaw = fmod(yaw + M_PI, 2 * M_PI) - M_PI;//mapTbaselink_yaw

    //=============================================

    //todo 1. mapTbaselink
    tf::Transform mapTbaselink_transform;
    mapTbaselink_transform.setOrigin(tf::Vector3(mapTbaselink_pose.position.x, mapTbaselink_pose.position.y, 0.0));
    // tf::Quaternion q1(laserTscan_rx, laserTscan_ry, laserTscan_rz, laserTscan_rw); //  x, y, z，w, 分量
    // laserTscanPoint_transform.setRotation(q1); laser_to_base_link_transform_
    tf::Quaternion mapTbaselink_q;
    mapTbaselink_q.setRPY(0, 0, mapTbaselink_pose.position.yaw);
    mapTbaselink_transform.setRotation(mapTbaselink_q);


    if( test_num_==42)
        tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbaselink_transform, ros::Time::now(), map_frame_id_, "trilateration_with_yaw_new_mapTbaselink_transform_transform_trilateration_with_yaw_new"));

    std::cout<<"trilateration_with_yaw_new: -----------------------------------------mapTbaselink_pose.position.x------"<<mapTbaselink_pose.position.x<<std::endl;
    std::cout<<"trilateration_with_yaw_new: -----------------------------------------mapTbaselink_pose.position.y-----"<<mapTbaselink_pose.position.y<<std::endl;
    std::cout<<"trilateration_with_yaw_new: -----------------------------------------mapTbaselink_pose.position.yaw-----"<<mapTbaselink_pose.position.yaw<<std::endl;
    return mapTbaselink_pose;
}

//经典三边定位最小二乘法 (法一) 临时变量，目前 scan_CB_new 中用的 2024-08-19  7209
Pose trilateration_with_yaw_new(const std::vector<ReflectorPanelPointStruct> &structIndices_first) {

//    （法一）
//    参考： /home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/image/baselink姿态计算.png
//    已知两个反光柱a,b分别在map坐标系的坐标A，B及在baselink中的坐标a,b。[不使用2个反光柱，用一个反光柱和一个 baselink 计算姿态也可以的]
//    ->                       ->
//    分别计算 AB 向量在map中的姿态YAW，与 ab 向量在 baselink 中的姿态yaw。
//    baselink在map中的姿态Yaw=YAW-yaw。


    //  std::this_thread::sleep_for(std::chrono::milliseconds(10));
    int n = structIndices_first.size();
    // 输入数据验证
    if (!areDistancesValid(structIndices_first)) {
        // throw invalid_argument("Invalid input: Number of anchors must match number of distances and all distances must be positive.");
        std::cerr<<"4411 经典三边定位最小二乘法 (法一) 临时变量 trilateration_with_yaw_new Invalid input: Number of anchors must match number of distances and all distances must be positive." <<std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        Pose pose;
        return pose;
    }
    //初始化，定义一个n行3列的矩阵A，用于存储系数。
    MatrixXd A(n, 3);
    //VectorXd b(n); 定义一个n维的列向量b，用于存储常数项。
    VectorXd b(n);
    //数据准备，通过循环遍历reflectorPanelPoint
    for (int i = 0; i < n; ++i) {
        // std::this_thread::sleep_for(std::chrono::milliseconds(10));
        //这是一个包含ReflectorPanelPoint类型的指针数组或容器，每个元素指向一个reflectorPanelPoints类
        auto ref =  structIndices_first[i];
        //反射点的在map下的坐标(x1, y1)
        double x1 = structIndices_first[i].x;
        double y1 = structIndices_first[i].y;

        //反光板到 base_link 的距离 [基于雷达探测到的数据] 【定位丢失时这个数据是可靠的（未涉及到了map坐标系， 是基于 lase - > baselink 计算的）】
        double d3 = structIndices_first[i].base_linkTfanguangban_distances;//【法二计算姿态】
//        std::cerr<<"3781 经典三边定位最小二乘法 (法一):trilateration_with_yaw_new,id: " <<reflectorPanelPoints[i]->id<<std::endl;
//        std::cerr<<"x1: " <<x1<<std::endl;
//        std::cerr<<"y1: " <<y1<<std::endl;
//        std::cerr<<"base_linkTfanguangban_distances: " <<d3<<std::endl;

        //构建线性系统，计算矩阵A的第i行，这形成了一个关于未知点坐标(x, y)的线性方程组，其中每个方程来源于一个反射点到估计点距离的平方与实际测量距离平方之差
        A(i, 0) = -2 * x1;
        A(i, 1) = -2 * y1;
        A(i, 2) = 1;
        //构建线性系统，计算向量b的第i个元素，即每个反射点坐标平方和减去测量距离平方。
        b(i) = x1 * x1 + y1 * y1 - d3 * d3;//得到 baselink的坐标

    }

    //解线性系统，使用列主元Householder QR分解方法求解线性方程组Ax=c。
    Vector3d mapTbaselink_result = A.colPivHouseholderQr().solve(b);//baselink 的坐标
//----------------------------------------------------------------------

    Pose mapTbaselink_pose;//baselink的坐标
    //todo sukai  2024-07-12 取反，发现数据符号相反；
    mapTbaselink_pose.position.x=-mapTbaselink_result(0);//取反
    mapTbaselink_pose.position.y=-mapTbaselink_result(1);//取反
    mapTbaselink_pose.id=map_frame_id_;//TODO  计算得到的是  小车 base_link 在map下 的坐标



//    std::cerr<<"mapTbaselink_pose.position.x: " <<mapTbaselink_pose.position.x<<std::endl;
//    std::cerr<<"mapTbaselink_pose.position.y: " <<mapTbaselink_pose.position.y<<std::endl;
//
//
//    std::cerr<<"---------------------------------------------------------" <<std::endl;
//    std::cerr<<"reflectorPanelPoints[0]->x: " <<reflectorPanelPoints[0]->x<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->x: " <<reflectorPanelPoints[1]->x<<std::endl;
//
//    std::cerr<<"reflectorPanelPoints[0]->y: " <<reflectorPanelPoints[0]->y<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->y: " <<reflectorPanelPoints[1]->y<<std::endl;
//    std::cerr<<"---------------------------------------------------------" <<std::endl;
//    std::cerr<<"reflectorPanelPoints[0]->current_baselinkTscan_x: " <<reflectorPanelPoints[0]->current_baselinkTscan_x<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->current_baselinkTscan_x: " <<reflectorPanelPoints[1]->current_baselinkTscan_x<<std::endl;
//
//    std::cerr<<"reflectorPanelPoints[0]->current_baselinkTscan_y: " <<reflectorPanelPoints[0]->current_baselinkTscan_y<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->current_baselinkTscan_y: " <<reflectorPanelPoints[1]->current_baselinkTscan_y<<std::endl;


    //以下这段代码假设 result(0)和result(1) 是估计点的x和y坐标，且之前已经正确解决了线性系统。此外，计算的偏航角差值可能需要根据实际情况进行调整，例如考虑坐标系统的定义和旋转的方向。
    // 改进偏航角计算，考虑使用第一个和第二个锚点的方向作为参考
    //这里计算了从第一个反射点到第二个反射点的向量（refX和refY），这代表了参考方向。
    double refX = structIndices_first[1].x - structIndices_first[0].x;//yaw2 -map
    double refY = structIndices_first[1].y - structIndices_first[0].y;//yaw2 -map

    //反光柱的在laser下的坐标(laserTscan_x, laserTscan_y)
//    double scan_x1 = reflectorPanelPoints[1]->laserTscan_x - reflectorPanelPoints[0]->laserTscan_x;//yaw1 -baselink
//    double scan_y1 = reflectorPanelPoints[1]->laserTscan_y - reflectorPanelPoints[0]->laserTscan_y;//yaw1 -baselink
    //反光柱的在baselink下的坐标(current_baselinkTscan_x, current_baselinkTscan_y)
    double scan_x1 = structIndices_first[1].current_baselinkTscan_x - structIndices_first[0].current_baselinkTscan_x;//yaw1 -baselink
    double scan_y1 = structIndices_first[1].current_baselinkTscan_y - structIndices_first[0].current_baselinkTscan_y;//yaw1 -baselink

    //计算参考方向的偏航角: yaw_ref=atan2(refY, refX); 计算估计点的偏航角:yaw_est = atan2(result(1), result(0));。atan2函数返回的范围是[-π, π]
    //  double yaw =  atan2(scan_x1, scan_y1)-atan2(refY, refX);
    double yaw2  = atan2(refY, refX); // 向量在 map 坐标系中的姿态
    double yaw1  = atan2(scan_y1, scan_x1);// 向量在 baselink 坐标系中的姿态
    double yaw =  yaw2-yaw1;//小车baselink的姿态=向量在 map 坐标系中的姿态-向量在 baselink 坐标系中的姿态 【 yaw = yaw2 - yaw1 】
    //计算偏航角差值: yaw = yaw_ref - yaw_est; 这里计算了参考方向的偏航角与估计点的偏航角之间的差值；                  atan2函数返回的范围是[-π, π] ，确保偏航角范围在[-pi, pi]
    //这段代码确保yaw的值在[-π, π]范围内。fmod函数用于取模运算，这里的目的是将偏航角差值映射回标准的2π周期内。首先加上π，然后对2π取模，最后减去π，确保结果在[-π, π]之间。
    mapTbaselink_pose.position.yaw = fmod(yaw + M_PI, 2 * M_PI) - M_PI;//mapTbaselink_yaw

    //=============================================

    //todo 1. mapTbaselink
    tf::Transform mapTbaselink_transform;
    mapTbaselink_transform.setOrigin(tf::Vector3(mapTbaselink_pose.position.x, mapTbaselink_pose.position.y, 0.0));
    // tf::Quaternion q1(laserTscan_rx, laserTscan_ry, laserTscan_rz, laserTscan_rw); //  x, y, z，w, 分量
    // laserTscanPoint_transform.setRotation(q1); laser_to_base_link_transform_
    tf::Quaternion mapTbaselink_q;
    mapTbaselink_q.setRPY(0, 0, mapTbaselink_pose.position.yaw);
    mapTbaselink_transform.setRotation(mapTbaselink_q);


    if( test_num_==42)
        tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbaselink_transform, ros::Time::now(), map_frame_id_, "trilateration_with_yaw_new_mapTbaselink_transform_transform_trilateration_with_yaw_new"));

    std::cout<<"trilateration_with_yaw_new: -----------------------------------------mapTbaselink_pose.position.x------"<<mapTbaselink_pose.position.x<<std::endl;
    std::cout<<"trilateration_with_yaw_new: -----------------------------------------mapTbaselink_pose.position.y-----"<<mapTbaselink_pose.position.y<<std::endl;
    std::cout<<"trilateration_with_yaw_new: -----------------------------------------mapTbaselink_pose.position.yaw-----"<<mapTbaselink_pose.position.yaw<<std::endl;
    return mapTbaselink_pose;
}



//经典三边定位最小二乘法 (法一)
// (法一)计算姿态方式看图片

Pose trilateration_with_yaw_new(const std::vector<std::shared_ptr<ReflectorPanelPoint>> &reflectorPanelPoints) {

//    （法一）
//    参考： /home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/image/baselink姿态计算.png
//    已知两个反光柱a,b分别在map坐标系的坐标A，B及在baselink中的坐标a,b。[不使用2个反光柱，用一个反光柱和一个 baselink 计算姿态也可以的]
//    ->                       ->
//    分别计算 AB 向量在map中的姿态YAW，与 ab 向量在 baselink 中的姿态yaw。
//    baselink在map中的姿态Yaw=YAW-yaw。


    //  std::this_thread::sleep_for(std::chrono::milliseconds(10));
    int n = reflectorPanelPoints.size();
    // 输入数据验证
    if (!areDistancesValid(reflectorPanelPoints)) {
        // throw invalid_argument("Invalid input: Number of anchors must match number of distances and all distances must be positive.");
        std::cerr<<"Invalid input: Number of anchors must match number of distances and all distances must be positive." <<std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        Pose pose;
        return pose;
    }
    //初始化，定义一个n行3列的矩阵A，用于存储系数。
    MatrixXd A(n, 3);
    //VectorXd b(n); 定义一个n维的列向量b，用于存储常数项。
    VectorXd b(n);
    //数据准备，通过循环遍历reflectorPanelPoint
    for (int i = 0; i < n; ++i) {
        // std::this_thread::sleep_for(std::chrono::milliseconds(10));
        //这是一个包含ReflectorPanelPoint类型的指针数组或容器，每个元素指向一个reflectorPanelPoints类
        auto ref =  reflectorPanelPoints[i];
        //反射点的在map下的坐标(x1, y1)
        double x1 = reflectorPanelPoints[i]->x;
        double y1 = reflectorPanelPoints[i]->y;

        //反光板到 base_link 的距离 [基于雷达探测到的数据] 【定位丢失时这个数据是可靠的（未涉及到了map坐标系， 是基于 lase - > baselink 计算的）】
        double d3 = reflectorPanelPoints[i]->base_linkTfanguangban_distances;//【法二计算姿态】
//        std::cerr<<"3781 经典三边定位最小二乘法 (法一):trilateration_with_yaw_new,id: " <<reflectorPanelPoints[i]->id<<std::endl;
//        std::cerr<<"x1: " <<x1<<std::endl;
//        std::cerr<<"y1: " <<y1<<std::endl;
//        std::cerr<<"base_linkTfanguangban_distances: " <<d3<<std::endl;

        //构建线性系统，计算矩阵A的第i行，这形成了一个关于未知点坐标(x, y)的线性方程组，其中每个方程来源于一个反射点到估计点距离的平方与实际测量距离平方之差
        A(i, 0) = -2 * x1;
        A(i, 1) = -2 * y1;
        A(i, 2) = 1;
        //构建线性系统，计算向量b的第i个元素，即每个反射点坐标平方和减去测量距离平方。
        b(i) = x1 * x1 + y1 * y1 - d3 * d3;//得到 baselink的坐标

    }

    //解线性系统，使用列主元Householder QR分解方法求解线性方程组Ax=c。
    Vector3d mapTbaselink_result = A.colPivHouseholderQr().solve(b);//baselink 的坐标
//----------------------------------------------------------------------

    Pose mapTbaselink_pose;//baselink的坐标
    //todo sukai  2024-07-12 取反，发现数据符号相反；
    mapTbaselink_pose.position.x=-mapTbaselink_result(0);//取反
    mapTbaselink_pose.position.y=-mapTbaselink_result(1);//取反
    mapTbaselink_pose.id=map_frame_id_;//TODO  计算得到的是  小车 base_link 在map下 的坐标

//    std::cerr<<"mapTbaselink_pose.position.x: " <<mapTbaselink_pose.position.x<<std::endl;
//    std::cerr<<"mapTbaselink_pose.position.y: " <<mapTbaselink_pose.position.y<<std::endl;
//
//
//    std::cerr<<"---------------------------------------------------------" <<std::endl;
//    std::cerr<<"reflectorPanelPoints[0]->x: " <<reflectorPanelPoints[0]->x<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->x: " <<reflectorPanelPoints[1]->x<<std::endl;
//
//    std::cerr<<"reflectorPanelPoints[0]->y: " <<reflectorPanelPoints[0]->y<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->y: " <<reflectorPanelPoints[1]->y<<std::endl;
//    std::cerr<<"---------------------------------------------------------" <<std::endl;
//    std::cerr<<"reflectorPanelPoints[0]->current_baselinkTscan_x: " <<reflectorPanelPoints[0]->current_baselinkTscan_x<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->current_baselinkTscan_x: " <<reflectorPanelPoints[1]->current_baselinkTscan_x<<std::endl;
//
//    std::cerr<<"reflectorPanelPoints[0]->current_baselinkTscan_y: " <<reflectorPanelPoints[0]->current_baselinkTscan_y<<std::endl;
//    std::cerr<<"reflectorPanelPoints[1]->current_baselinkTscan_y: " <<reflectorPanelPoints[1]->current_baselinkTscan_y<<std::endl;


    //以下这段代码假设 result(0)和result(1) 是估计点的x和y坐标，且之前已经正确解决了线性系统。此外，计算的偏航角差值可能需要根据实际情况进行调整，例如考虑坐标系统的定义和旋转的方向。
    // 改进偏航角计算，考虑使用第一个和第二个锚点的方向作为参考
    //这里计算了从第一个反射点到第二个反射点的向量（refX和refY），这代表了参考方向。
    double refX = reflectorPanelPoints[1]->x - reflectorPanelPoints[0]->x;//yaw2 -map
    double refY = reflectorPanelPoints[1]->y - reflectorPanelPoints[0]->y;//yaw2 -map

    //反光柱的在laser下的坐标(laserTscan_x, laserTscan_y)
//    double scan_x1 = reflectorPanelPoints[1]->laserTscan_x - reflectorPanelPoints[0]->laserTscan_x;//yaw1 -baselink
//    double scan_y1 = reflectorPanelPoints[1]->laserTscan_y - reflectorPanelPoints[0]->laserTscan_y;//yaw1 -baselink
    //反光柱的在baselink下的坐标(current_baselinkTscan_x, current_baselinkTscan_y)
    double scan_x1 = reflectorPanelPoints[1]->current_baselinkTscan_x - reflectorPanelPoints[0]->current_baselinkTscan_x;//yaw1 -baselink
    double scan_y1 = reflectorPanelPoints[1]->current_baselinkTscan_y - reflectorPanelPoints[0]->current_baselinkTscan_y;//yaw1 -baselink

    //计算参考方向的偏航角: yaw_ref=atan2(refY, refX); 计算估计点的偏航角:yaw_est = atan2(result(1), result(0));。atan2函数返回的范围是[-π, π]
    //  double yaw =  atan2(scan_x1, scan_y1)-atan2(refY, refX);
    double yaw2  = atan2(refY, refX); // 向量在 map 坐标系中的姿态
    double yaw1  = atan2(scan_y1, scan_x1);// 向量在 baselink 坐标系中的姿态
    double yaw =  yaw2-yaw1;//小车baselink的姿态=向量在 map 坐标系中的姿态-向量在 baselink 坐标系中的姿态 【 yaw = yaw2 - yaw1 】
    //计算偏航角差值: yaw = yaw_ref - yaw_est; 这里计算了参考方向的偏航角与估计点的偏航角之间的差值；                  atan2函数返回的范围是[-π, π] ，确保偏航角范围在[-pi, pi]
    //这段代码确保yaw的值在[-π, π]范围内。fmod函数用于取模运算，这里的目的是将偏航角差值映射回标准的2π周期内。首先加上π，然后对2π取模，最后减去π，确保结果在[-π, π]之间。
    mapTbaselink_pose.position.yaw = fmod(yaw + M_PI, 2 * M_PI) - M_PI;//mapTbaselink_yaw

    //=============================================

    //todo 1. mapTbaselink
    tf::Transform mapTbaselink_transform;
    mapTbaselink_transform.setOrigin(tf::Vector3(mapTbaselink_pose.position.x, mapTbaselink_pose.position.y, 0.0));
    // tf::Quaternion q1(laserTscan_rx, laserTscan_ry, laserTscan_rz, laserTscan_rw); //  x, y, z，w, 分量
    // laserTscanPoint_transform.setRotation(q1); laser_to_base_link_transform_
    tf::Quaternion mapTbaselink_q;
    mapTbaselink_q.setRPY(0, 0, mapTbaselink_pose.position.yaw);
    mapTbaselink_transform.setRotation(mapTbaselink_q);


    if( test_num_==42)
        tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbaselink_transform, ros::Time::now(), map_frame_id_, "mapTbaselink_transform_transform_trilateration_with_yaw_new"));


    return mapTbaselink_pose;
}




//经典三边定位最小二乘法 (法二) 6102
//(法二)计算方式是我们已经通过最小二乘得到了小车在地图中的位置或者是雷达在地图中的位置，又因为我们知道雷达与小车的安装坐标，所以可以求解出一个未知量，通过小车坐标与雷达坐标可以得到一条向量，通过计算向量在地图中的角度就能得到姿态了。

Pose trilateration_with_yaw(const std::vector<std::shared_ptr<ReflectorPanelPoint>> &reflectorPanelPoints) {

    // 通过雷达扫描到的反光柱的距离数据，移动机器人中心点baselink坐标以及相对的雷达的安装坐标，我们可计算得到反光柱到baselink的距离，通过着两个距离 分别用三边定位最小二乘法计算出 移动机器人中心点baselink在map中的坐标以及雷达laser在map中的安装坐标。
    // 通过着两个坐标可以拟合一条向量，通过这条向量计算得到姿态。当然我么也可以使用移动机器人身上的其它点为来计算，因为机器人是刚性的，已知机器人身上的任意一个坐标，我们就能计算得到其他任意坐标

    int n = reflectorPanelPoints.size();

    // 输入数据验证
    if (!areDistancesValid(reflectorPanelPoints)) {
        // throw invalid_argument("Invalid input: Number of anchors must match number of distances and all distances must be positive.");
        std::cerr<<"Invalid input: Number of anchors must match number of distances and all distances must be positive." <<std::endl;

        Pose pose;
        return pose;
    }
    //初始化，定义一个n行3列的矩阵A，用于存储系数。
    MatrixXd A(n, 3);
    //VectorXd b(n); 定义一个n维的列向量b，用于存储常数项。
    VectorXd b(n);

    VectorXd c(n);
    //数据准备，通过循环遍历reflectorPanelPoint
    for (int i = 0; i < n; ++i) {
        //这是一个包含ReflectorPanelPoint类型的指针数组或容器，每个元素指向一个reflectorPanelPoints类
        auto ref =  reflectorPanelPoints[i];
        //反射点的在map下的坐标(x1, y1)
        double x1 = reflectorPanelPoints[i]->x;
        double y1 = reflectorPanelPoints[i]->y;
        //反光板到 base_link 的距离 [通过坐标变换计算的到的距离，涉及到了map，所以这个距离是依赖先有准确定位才行]
        // double d1 = reflectorPanelPoints[i]->distances;//用不到
        //反光板到 laser 的距离 [基于雷达探测到的数据，定位丢失时这个数据是可靠的]
        // 加上反光柱的半径
        // d2 =reflectorPanelPoints[i]->range+ fangaungzhuRadius_;
        double d2 = reflectorPanelPoints[i]->range;

        //反光板到 base_link 的距离 [基于雷达探测到的数据] 【定位丢失时这个数据是可靠的（未涉及到了map坐标系， 是基于 lase - > baselink 计算的）】
        double d3 = reflectorPanelPoints[i]->base_linkTfanguangban_distances;//【法二计算姿态】
        //【效果不明显 暂时不用】反光板到 mind 的距离， mind是 base_link 与 laser两点连线的延长线。[基于雷达探测到的数据]【定位丢失时这个数据是可靠的（未涉及到了map坐标系， 是基于 lase - > baselink 计算的）】
        // double d3 = reflectorPanelPoints[i]-> mindTfanguangban_distances; //效果不明显 暂时不用

//           std::cout<<reflectorPanelPoints[i]->id<<" : ----2949 到某固定点的距离d-----------reflectorPanelPoints[i]->distances-------------------d1: "<<d1 <<std::endl;
//            std::cout<<reflectorPanelPoints[i]->id<<" : ----2949 到某固定点的距离d-----------reflectorPanelPoints[i]->range-------------------d2: "<<d2 <<std::endl;
//        std::cout<<reflectorPanelPoints[i]->id<<" : ----2949 到某固定点的距离d-----------reflectorPanelPoints[i]->base_linkTfanguangban_distances-------------------d3: "<<d3 <<std::endl;
//        std::cout<<reflectorPanelPoints[i]->id<<" : ----2949 到某固定点的距离d-----------reflectorPanelPoints[i]->base_linkTfanguangban_distances-------------------d3: "<<d3 <<std::endl;
//        std::cout<<reflectorPanelPoints[i]->id<<" : ----2949 到某固定点的距离d-----------reflectorPanelPoints[i]->base_linkTfanguangban_distances-------------------reflectorPanelPoints[i]->laserTscan_x: "<< reflectorPanelPoints[i]->laserTscan_x <<std::endl;
//        std::cout<<reflectorPanelPoints[i]->id<<" : ----2949 到某固定点的距离d-----------reflectorPanelPoints[i]->base_linkTfanguangban_distances-------------------reflectorPanelPoints[i]->laserTscan_y: "<<reflectorPanelPoints[i]->laserTscan_y <<std::endl;


        //构建线性系统，计算矩阵A的第i行，这形成了一个关于未知点坐标(x, y)的线性方程组，其中每个方程来源于一个反射点到估计点距离的平方与实际测量距离平方之差
        A(i, 0) = -2 * x1;
        A(i, 1) = -2 * y1;
        A(i, 2) = 1;
        //构建线性系统，计算向量b的第i个元素，即每个反射点坐标平方和减去测量距离平方。
        // b(i) = x1 * x1 + y1 * y1 - d1 * d1;
        b(i) = x1 * x1 + y1 * y1 - d2 * d2;//得到 雷达的坐标
        c(i) = x1 * x1 + y1 * y1 - d3 * d3;//得到 baselink的坐标
    }
    //解线性系统，使用列主元Householder QR分解方法求解线性方程组Ax=b。
    Vector3d mapTlaser_result = A.colPivHouseholderQr().solve(b);//雷达的坐标
    //解线性系统，使用列主元Householder QR分解方法求解线性方程组Ax=c。
    Vector3d mapTbaselink_result = A.colPivHouseholderQr().solve(c);//baselink 的坐标
    //结果result是一个三维向量，包含估计的点坐标(x, y)及可能的残差或无关的第三维度（取决于实际问题背景，但通常表示为误差项）。
    Point point_result;//提取结果，将解得的坐标赋值
    //即估计点的x和y坐标分别等于解向量的第一个和第二个元素。
    point_result.x=mapTlaser_result(0);
    point_result.y=mapTlaser_result(1);

//    double refX = reflectorPanelPoints[1]->x - reflectorPanelPoints[0]->x;
//    double refY = reflectorPanelPoints[1]->y - reflectorPanelPoints[0]->y;
//    double average_yaw = atan2(refY, refX) - atan2(mapTlaser_result(1), mapTlaser_result(0));

    //反射点的在baselink下的坐标(x1, y1)
    double scan_x1 = reflectorPanelPoints[1]->laserTscan_x - reflectorPanelPoints[0]->laserTscan_x;//yaw1 -baselink
    double scan_y1 = reflectorPanelPoints[1]->laserTscan_y - reflectorPanelPoints[0]->laserTscan_y;//yaw1 -baselink
    double yaw1  = atan2(scan_y1, scan_x1);// 向量在 baselink 坐标系中的姿态
//----------------------------------------------------------------------
    Pose mapTbaselink_pose;//baselink的坐标
    //todo sukai  2024-07-12 取反，发现数据符号相反；
    mapTbaselink_pose.position.x=-mapTbaselink_result(0);//取反
    mapTbaselink_pose.position.y=-mapTbaselink_result(1);//取反

    mapTbaselink_pose.id=base_link_frame_id_;//TODO  计算得到的是  小车 base_link 在map下 的坐标

    Pose mapTlaser_pose;//雷达的坐标
    //todo sukai  2024-07-12 取反，发现数据符号相反；
    mapTlaser_pose.position.x= -mapTlaser_result(0);//取反
    mapTlaser_pose.position.y= -mapTlaser_result(1);//取反
    mapTlaser_pose.id=laser_frame_id_;//TODO  计算得到的是  小车 base_link 在map下 的坐标
    // 计算方向向量
    double dx = mapTbaselink_pose.position.x - mapTlaser_pose.position.x;
    double dy = mapTbaselink_pose.position.y - mapTlaser_pose.position.y;
    // 计算 base_link 的 yaw 角度
    double base_link_yaw = atan2(dy, dx);//效果还可以

    mapTbaselink_pose.position.yaw=base_link_yaw;
    mapTlaser_pose.position.yaw=base_link_yaw;


    return mapTbaselink_pose;
}




//===========================================经典三边定位最小二乘法 test17 ====================================================














//===========================================卡尔曼滤波器目标点跟踪============ test19 ========================================






//============================================================================



// 计算点云的几何中心  todo 暂时没用到
Point computeCentroid(const std::vector<Point>& points) {
    double sum_x = 0, sum_y = 0;
    for (const auto& point : points) {
        sum_x += point.x;
        sum_y += point.y;

//        sum_x += point.laserTscan_x;
//        sum_y += point.laserTscan_y;

    }
    double points_size=(double)points.size();
    Point centroid;
    centroid.x=sum_x / points_size;
    centroid.y=sum_y / points_size;
//    centroid.laserTscan_x=sum_x / points_size;
//    centroid.laserTscan_y=sum_y / points_size;

    return centroid;
}



void fitCircle(const std::vector<Point>& points, Point& center, double& radius) {
    int n = points.size();
    double sum_x = 0, sum_y = 0, sum_xx = 0, sum_yy = 0, sum_xy = 0, sum_x1y2 = 0, sum_x2y1 = 0;

    for (const auto& p : points) {
        double x = p.laserTscan_x;
        double y = p.laserTscan_y;
        double xx = x * x;
        double yy = y * y;
        sum_x += x;
        sum_y += y;
        sum_xx += xx;
        sum_yy += yy;
        sum_xy += x * y;
        sum_x1y2 += x * yy;
        sum_x2y1 += xx * y;
    }

    double C = n * sum_xx - sum_x * sum_x;
    double D = n * sum_xy - sum_x * sum_y;
    double E = n * sum_x1y2 + n * sum_x2y1 - (sum_xx + sum_yy) * sum_y;
    double G = n * sum_yy - sum_y * sum_y;
    double H = n * sum_x1y2 + n * sum_x2y1 - (sum_xx + sum_yy) * sum_x;

    double denominator = (C * G - D * D);
    if (std::fabs(denominator) < 1e-10) {
        std::cerr << "Denominator is too small, causing instability.\n";
        //center = {0, 0};
        center.laserTscan_x=0;
        center.laserTscan_y=0;
        radius = 0;
        return;
    }

    double a = (H * D - E * G) / denominator;
    double b = (H * C - E * D) / (D * D - G * C);
    double c = -(sum_x * a + sum_y * b + sum_xx + sum_yy) / n;

    center.laserTscan_x = a / -2;
    center.laserTscan_y = b / -2;
    radius = std::sqrt(a * a + b * b - 4 * c) / 2;
}




//----------------------------------------------------------

//--------------------------判断是否是直线--------------------------
bool isCircular(const std::vector<Point>& points, double threshold) {
    if (points.size() < 5) {
        return false; // 不足3个点无法构成圆
    }

    Point center;
    double radius;
    fitCircle(points, center, radius);

    // std::cout << "Fitted Circle Center: (" << center.laserTscan_x << ", " << center.laserTscan_y << ")\n";
    // std::cout << "Fitted Circle Radius: " << radius << "\n";

    double totalError = 0;
    for (const auto& point : points) {
        double distance = std::sqrt(std::pow(point.laserTscan_x - center.laserTscan_x, 2) + std::pow(point.laserTscan_y - center.laserTscan_y, 2));
        totalError += std::fabs(distance - radius);
    }

    double meanError = totalError / points.size();
    //   std::cout << "isCircular Mean Error: " << meanError << "\n";

    return meanError < threshold;
}


//发布标记
void Publish(ros::Publisher  &pub_marker,visualization_msgs::Marker &marker){
//    while (pub_marker.getNumSubscribers() < 1) {
//       // sleep(1);
//        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
//    }
    marker.header.stamp = ros::Time();
    pub_marker.publish(marker);
}
//---------------------------------------------------------------------------


void fitLine(const std::vector<Point>& points, double& slope, double& intercept) {
    int n = points.size();
    double sum_x = 0, sum_y = 0, sum_xy = 0, sum_xx = 0;

    for (const auto& point : points) {
        sum_x += point.laserTscan_x;
        sum_y += point.laserTscan_y;
        sum_xy += point.laserTscan_x * point.laserTscan_y;
        sum_xx += point.laserTscan_x * point.laserTscan_x;
    }

    double denominator = n * sum_xx - sum_x * sum_x;
    if (std::abs(denominator) < 1e-10) {
        std::cerr << "Denominator is too small, causing instability.\n";
        slope = 0;
        intercept = 0;
        return;
    }

    slope = (n * sum_xy - sum_x * sum_y) / denominator;
    intercept = (sum_y - slope * sum_x) / n;
}




bool isLine(const std::vector<Point>& points, double threshold) {
    if (points.size() < 2) {
        return false; // 不足2个点无法构成直线
    }

    double slope, intercept;
    fitLine(points, slope, intercept);

    //  std::cout << "Fitted Line: y = " << slope << "x + " << intercept << "\n";

    double totalError = 0;
    for (const auto& point : points) {
        double predicted_y = slope * point.laserTscan_x + intercept;
        totalError += std::abs(predicted_y - point.laserTscan_y);
    }

    double meanError = totalError / points.size();
    // std::cout << "isLine Mean Error: " << meanError << "\n";

    return meanError < threshold;
}



//============================================================
// 从points中取出中间一个数据，主要针对反光板
Point getMiddlePoint(const std::vector<Point>& points) {
    if (points.empty()) {
        // 如果points为空，返回一个默认的 Point
        Point point;
        return point; // 这里的{0, 0}是一个默认的Point，具体根据你的需求来设置
    }

    // 计算中间索引
    int middle_index = points.size() / 2;

    // 返回中间一个数据
    return points[middle_index];
}
//===============================================================





//------------------------------------------------------
std::vector<Point> movingAverageFilter(const std::vector<Point> &points, int window_size) {



    // 创建可修改的副本
    std::vector<Point> filtered_points = points;



    // 处理 points 的数量
    if (points.size() == 4) {
        // 如果 points 的大小是 4，跳过第一个点
        for (size_t i = 1; i < points.size(); ++i) {
            filtered_points.push_back(points[i]);
        }
    } else if (points.size() > 4) {
        // 如果 points 的大小大于 4，跳过首尾点
        for (size_t i = 1; i < points.size() - 1; ++i) {
            filtered_points.push_back(points[i]);
        }
    } else {
        // 如果点的数量小于或等于4，直接使用原始数据
        filtered_points = points;
    }

    if(points.size()<3)
        return filtered_points;

    std::vector<Point> smoothed_points = filtered_points;
    // 确保有足够的点进行滤波
    if (smoothed_points.size() < window_size || window_size < 1) {
        return smoothed_points;  // 数据量太小，无法进行滤波，直接返回原始数据
    }

    int half_window = window_size / 2;

    for (size_t i = half_window; i < points.size() - half_window; ++i) {
        double sum_x = 0;
        double sum_y = 0;
        double sum_range = 0;

        // 滑动窗口内求平均
        for (int j = -half_window; j <= half_window; ++j) {
            sum_x += filtered_points[i + j].laserTscan_x;
            sum_y += filtered_points[i + j].laserTscan_y;
            sum_range += filtered_points[i + j].range;
        }

        // 平均化窗口内的值
        smoothed_points[i].laserTscan_x = sum_x / window_size;
        smoothed_points[i].laserTscan_y = sum_y / window_size;
        smoothed_points[i].range = sum_range / window_size;
    }

    return smoothed_points;
}






//------------------------------------------------------
std::vector<Point> movingAverageFilter(const std::vector<Point> &points, int window_size, int sampling_rate) {
    // 创建结果向量
    std::vector<Point> filtered_points;
    std::vector<Point> filtered_points1;
    // 如果点云数量过少，直接返回原始点云
    if (points.size() < window_size || window_size < 1 || sampling_rate < 1) {
        return points;  // 数据量太小，或参数无效，直接返回原始数据
    }

    // 遍历点云，进行稀疏采样
    for (size_t i = 0; i < points.size(); i += sampling_rate) {
        filtered_points.push_back(points[i]);  // 每隔 `sampling_rate` 个点采样一次
    }

    // 对采样后的点云进行平滑滤波
    std::vector<Point> smoothed_points = filtered_points;
    int half_window = window_size / 2;

    // 确保有足够的点进行滤波
    if (filtered_points.size() < window_size) {
        return filtered_points;  // 如果点数少于窗口大小，无法进行滤波，直接返回
    }

    for (size_t i = half_window; i < filtered_points.size() - half_window; ++i) {
        double sum_x = 0;
        double sum_y = 0;
        double sum_range = 0;

        // 滑动窗口内求平均
        for (int j = -half_window; j <= half_window; ++j) {
            sum_x += filtered_points[i + j].laserTscan_x;
            sum_y += filtered_points[i + j].laserTscan_y;
            sum_range += filtered_points[i + j].range;
        }

        // 平均化窗口内的值
        smoothed_points[i].laserTscan_x = sum_x / window_size;
        smoothed_points[i].laserTscan_y = sum_y / window_size;
        smoothed_points[i].range = sum_range / window_size;
    }
//-------------------------------------------------------------------
    // 处理 points 的数量
    if (smoothed_points.size() == 4) {
        // 如果 points 的大小是 4，跳过第一个点
        for (size_t i = 1; i < smoothed_points.size(); ++i) {
            filtered_points1.push_back(smoothed_points[i]);
        }
    } else if (smoothed_points.size() > 4) {
        // 如果 points 的大小大于 4，跳过首尾点
        for (size_t i = 1; i < smoothed_points.size() - 1; ++i) {
            filtered_points1.push_back(smoothed_points[i]);
        }
    } else {
        // 如果点的数量小于或等于4，直接使用原始数据
        filtered_points1 = smoothed_points;
    }


    return smoothed_points;
}



////更有效的平均角度计算： 当前直接计算平均角度的方式可能会导致大角度偏差引发误差。可以考虑使用单位向量来平均角度：
Point trigonometry(std::vector<Point> &points) {
    double avg_cos = 0, avg_sin = 0;
    for (const auto &point : points) {
        avg_cos += cos(point.laserTscan_yaw);
        avg_sin += sin(point.laserTscan_yaw);
    }

    double avg_yaw = atan2(avg_sin / points.size(), avg_cos / points.size());
    double num = 0;

    for (const auto &point : points) {
        double os = point.laserTscan_yaw - avg_yaw;
        double ob = point.range * cos(os) + fangaungzhuRadius_ * cos(asin(point.range * sin(fabs(os)) / fangaungzhuRadius_));
        num += ob;
    }

    num /= points.size();

    Point trigon;
    trigon.laserTscan_yaw = avg_yaw;
    trigon.laserTscan_x = num * cos(avg_yaw);
    trigon.laserTscan_y = num * sin(avg_yaw);

    return trigon;
}
//-------------------------------------------------------

//9680 reflector_width：反光片的宽度，这里未被直接使用，因为我们假设反光片的宽度影响计算点的分布。
//radius_offset：这是人为设定的圆心与反光片中心的距离，确定圆心位于反光片后方的位置，避免因雷达不同角度扫描而导致坐标偏差。
Point trigonometryForReflector(std::vector<Point> &points, double reflector_width, double radius_offset) {
    // 1. 计算平均的cos和sin，来获取反光片的中心角度
    double avg_cos = 0, avg_sin = 0;
    for (const auto &point : points) {
        avg_cos += cos(point.laserTscan_yaw);
        avg_sin += sin(point.laserTscan_yaw);
    }

    double avg_yaw = atan2(avg_sin / points.size(), avg_cos / points.size());

    // 2. 计算反光片的中心坐标（根据雷达扫描点的平均位置）
    double avg_x = 0, avg_y = 0;
    for (const auto &point : points) {
        avg_x += point.laserTscan_x;
        avg_y += point.laserTscan_y;
    }
    avg_x /= points.size();
    avg_y /= points.size();

    // 3. 计算圆心位置（位于反光片后方）
    // 通过指定的半径偏移（radius_offset）来确定圆心相对反光片的偏移位置
    double circle_center_x = avg_x + radius_offset * cos(avg_yaw);
    double circle_center_y = avg_y + radius_offset * sin(avg_yaw);

    Point trigon;
    trigon.laserTscan_yaw = avg_yaw;            // 中心角度
    trigon.laserTscan_x = circle_center_x;       // 圆心X坐标
    trigon.laserTscan_y = circle_center_y;       // 圆心Y坐标

    // 4. 设置范围和其他相关参数
    trigon.range = sqrt(circle_center_x * circle_center_x + circle_center_y * circle_center_y);  // 计算距离
    trigon.intensity = 0;  // 强度设置为0，或者可以从数据中提取

    return trigon;
}

//----------------------------------------------------------
//设定一个曲率阈值。如果点的曲率平均值超过阈值，则认为是反光柱；否则认为是反光片。
bool checkCurvature(const std::vector<Point> &points) {


    // 确保有足够的点
    if (points.size() < 3) {
        return false;  // 不足三个点无法判断曲率
    }

    // 曲率的总和
    double total_curvature = 0;
    double total_ranges = 0;

    const Point &p1 = points[0];
    const Point &p2 = points[ points.size()/2];
    const Point &p3 = points[ points.size()-1];

    // 计算三个点之间的距离
    double L1 = sqrt(pow(p2.laserTscan_x - p1.laserTscan_x, 2) + pow(p2.laserTscan_y - p1.laserTscan_y, 2));
    double L2 = sqrt(pow(p3.laserTscan_x - p2.laserTscan_x, 2) + pow(p3.laserTscan_y - p2.laserTscan_y, 2));
    double L3 = sqrt(pow(p3.laserTscan_x - p1.laserTscan_x, 2) + pow(p3.laserTscan_y - p1.laserTscan_y, 2));

    // 使用海伦公式计算三角形的面积
    double s = (L1 + L2 + L3) / 2;
    double area = sqrt(s * (s - L1) * (s - L2) * (s - L3));
    total_ranges +=p1.range;
    total_ranges +=p2.range;
    total_ranges +=p3.range;



    // 计算平均曲率
    double avg_curvature = area;
    // 计算平均距离
    double avg_range = total_ranges / 3;

    //在阀值距离范围内，判断是反光柱还是反光板
    // double curvature_range_min_threshold_=1.5;//距离小于阀值有效
    //  double curvature_range_max_threshold_=2.6;//距离大于阀值有效
    if((avg_range<curvature_range_min_threshold_||avg_range>curvature_range_max_threshold_) && avg_curvature <= curvature_threshold_){


        if(time_log_fig_==24) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
            std::string resultType = "checkCurvature";
            std::string resultChineseMessage = " 设定一个曲率阈值。如果点的曲率平均值超过阈值，则认为是反光柱；否则认为是反光片, 曲率阈值:  curvature_threshold_： "+std::to_string(curvature_threshold_);
            std::string resultENMessage = "设定一个曲率阈值。如果点的曲率平均值超过阈值，则认为是反光柱；否则认为是反光片, 当前曲率值:  avg_curvature： "+std::to_string(avg_curvature)+" ,计算平均距离 avg_range： "+std::to_string(avg_range);
            std::string className = __FILE__; // 更改为实际的文件名
            int classLine = __LINE__; // 更改为实际的行号
            string matchReflectorPaneIds="EMPTY";
            // tf::Transform mapTbase_link_transformlog =    tf_last_odom_to_map_transform_*base_link_to_odom_transform_;
            geometry_msgs::Pose mapTbase_linklog;
            double mapTbase_link_yaw_log=0.0;
            ///trilateration_time_log
            publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size() ,matchReflectorPaneIds,mapTbase_linklog,mapTbase_link_yaw_log);

        }
        return false;
    }else{

        if(time_log_fig_==25) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
            std::string resultType = "checkCurvature";
            std::string resultChineseMessage = " 设定一个曲率阈值。如果点的曲率平均值超过阈值，则认为是反光柱；否则认为是反光片, 曲率阈值:  curvature_threshold_： "+std::to_string(curvature_threshold_);
            std::string resultENMessage = "设定一个曲率阈值。如果点的曲率平均值超过阈值，则认为是反光柱；否则认为是反光片, 当前曲率值:  avg_curvature： "+std::to_string(avg_curvature)+" ,计算平均距离 avg_range： "+std::to_string(avg_range);
            std::string className = __FILE__; // 更改为实际的文件名
            int classLine = __LINE__; // 更改为实际的行号
            string matchReflectorPaneIds="EMPTY";
            // tf::Transform mapTbase_link_transformlog =    tf_last_odom_to_map_transform_*base_link_to_odom_transform_;
            geometry_msgs::Pose mapTbase_linklog;
            double mapTbase_link_yaw_log=0.0;
            ///trilateration_time_log
            publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size() ,matchReflectorPaneIds,mapTbase_linklog,mapTbase_link_yaw_log);

        }

        return true;
    }



    // 如果平均曲率超过阈值，认为是反光柱
    return avg_curvature > curvature_threshold_;
}





// 计算反光柱的圆心坐标
Point calculateForColumn(std::vector<Point> &points, double radius_offset) {
    double avg_cos = 0, avg_sin = 0;
    for (const auto &point : points) {
        avg_cos += cos(point.laserTscan_yaw);
        avg_sin += sin(point.laserTscan_yaw);
    }
    double avg_yaw = atan2(avg_sin / points.size(), avg_cos / points.size());

    double avg_x = 0, avg_y = 0;
    for (const auto &point : points) {
        avg_x += point.laserTscan_x;
        avg_y += point.laserTscan_y;
    }
    avg_x /= points.size();
    avg_y /= points.size();

    double circle_center_x = avg_x + radius_offset * cos(avg_yaw);
    double circle_center_y = avg_y + radius_offset * sin(avg_yaw);

    Point trigon;
    trigon.laserTscan_yaw = avg_yaw;
    trigon.laserTscan_x = circle_center_x;
    trigon.laserTscan_y = circle_center_y;
    trigon.range = sqrt(circle_center_x * circle_center_x + circle_center_y * circle_center_y);

    return trigon;
}

// 计算反光片的中心坐标
Point calculateForBoard(std::vector<Point> &points, double reflector_width, double radius_offset) {
    // 假设第一个点和最后一个点为反光片的两端
    Point p1 = points.front();
    Point p2 = points.back();

    // 计算反光片的中心点
    double center_x = (p1.laserTscan_x + p2.laserTscan_x) / 2.0;
    double center_y = (p1.laserTscan_y + p2.laserTscan_y) / 2.0;

    // 计算反光片的角度（直线）
    double board_angle = atan2(p2.laserTscan_y - p1.laserTscan_y, p2.laserTscan_x - p1.laserTscan_x);

    // 计算圆心（位于反光片的后方，垂直于反光片方向）
    double circle_center_x = center_x + radius_offset * cos(board_angle - M_PI / 2);
    double circle_center_y = center_y + radius_offset * sin(board_angle - M_PI / 2);

    Point trigon;
    trigon.laserTscan_yaw = board_angle;
    trigon.laserTscan_x = circle_center_x;
    trigon.laserTscan_y = circle_center_y;
    trigon.range = sqrt(circle_center_x * circle_center_x + circle_center_y * circle_center_y);

    return trigon;
}



//区分了反光柱与反光板
Point trigonometryForReflectorNew(std::vector<Point> &points, double reflector_width, double radius_offset) {
    // 1. 通过曲率判断是反光柱还是反光片
    bool is_reflector_column = checkCurvature(points); // 假设有一个函数checkCurvature

    if (is_reflector_column) {
        // 如果是反光柱，继续使用现有逻辑来计算圆心
        return calculateForColumn(points, radius_offset);
        //  return trigonometry(points);//之前的
    } else {
        // 如果是反光片，进行直线计算并使用两端点计算中心
        return calculateForBoard(points, reflector_width, radius_offset);
    }
}


//-----------------------------------------------------------------
// 计算补偿后的圆心
//Eigen::Vector2d computeCircleCenterWithRadiusAndAngleCompensation(const std::vector<Point>& points) {
Point computeCircleCenterWithRadiusAndAngleCompensation(const std::vector<Point>& points) {
    MatrixXd A(points.size(), 3);// Eigen::MatrixXd A(points.size(), 3);
    VectorXd B(points.size());

    for (size_t i = 0; i < points.size(); i++) {
        double x = points[i].laserTscan_x;
        double y = points[i].laserTscan_y;

        // 使用入射角度进行权重计算
        double weight = 1.0; // 可以根据需要调整权重
        if (points[i].laserTscan_yaw != 0) {
            weight = 1.0 / std::cos(points[i].laserTscan_yaw); // 基于入射角度的简单补偿
        }

        A(i, 0) = weight * 2 * x;
        A(i, 1) = weight * 2 * y;
        A(i, 2) = weight; // 这里的权重用于补偿

        // 将半径影响加入到 B 向量中
        B(i) = weight * (x * x + y * y - fangaungzhuRadius_ * fangaungzhuRadius_);
    }


    Vector3d result = A.colPivHouseholderQr().solve(B);
    double cx =result(0), cy=result(1);
    // 计算距离
    double distance = sqrt(cx * cx + cy * cy);

    // 计算角度（以弧度为单位）
    double angle = atan2(cy, cx);
    Point center;
    center.range = distance;
    center.laserTscan_yaw = angle;

    center.laserTscan_x = cx;
    center.laserTscan_y = cy;
    center.laserTscan_z = 0.0;  // 假设在二维平面上

    // return Eigen::Vector2d(result(0), result(1)); // 返回圆心坐标
    return center; // 返回圆心坐标
}

//----------------------------------------------------------

//最小二乘法拟合圆心，根据公式原理编写用三角函数法拟定圆心的函数。2024-09-03
Point fitCircleCenter(const std::vector<Point>& points) {
    if (points.empty()) {
        return Point();  // 返回默认构造的点
    }

//    Eigen::MatrixXd A(points.size(), 2);
//    Eigen::VectorXd b(points.size());

    MatrixXd A(points.size(), 2);
    VectorXd b(points.size());

    // 构建线性方程组
    for (size_t i = 0; i < points.size(); ++i) {
        const auto& p = points[i];
        A(i, 0) = 2 * p.laserTscan_x;
        A(i, 1) = 2 * p.laserTscan_y;
        b(i) = p.laserTscan_x * p.laserTscan_x + p.laserTscan_y * p.laserTscan_y;
    }

    // 求解最小二乘问题
    //Eigen::VectorXd solution = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    VectorXd solution = A.bdcSvd(ComputeThinU | ComputeThinV).solve(b);
    // 计算圆心坐标
    double cx = -solution(0) / 2;
    double cy = -solution(1) / 2;

    Point center;
//    if(points.size()>1){
//        center.range = points[points.size()/2].range;
//        center.laserTscan_yaw = points[points.size()/2].laserTscan_yaw;
//    }

    // 计算距离
    double distance = sqrt(cx * cx + cy * cy);

    // 计算角度（以弧度为单位）
    double angle = atan2(cy, cx);

    center.range = distance;
    center.laserTscan_yaw = angle;

    center.laserTscan_x = cx;
    center.laserTscan_y = cy;
    center.laserTscan_z = 0.0;  // 假设在二维平面上

    return center;
}


//用于发布文本标记。 ros::Publisher  pub_all_marker_,pub_all_current_marker_;
void publishTextMarker(const double x, const double y,std::string &frame_id,std::string& datatxt,int markerid,
                       double & a,double & r,double & g,double &b,std::string &ns_id,double &lifetime,ros::Publisher & pub_marker) {
    visualization_msgs::Marker text_marker;
    text_marker.header.frame_id = frame_id;
    text_marker.header.stamp = ros::Time::now();
    text_marker.ns =ns_id ;//"target_tracking"
    text_marker.id = markerid;
    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;  //int 类型
    text_marker.action = visualization_msgs::Marker::ADD;

    text_marker.pose.position.x = x+0.05;
    text_marker.pose.position.y = y+0.05;
    text_marker.pose.position.z = 1.0;  // 高度稍微高一点，使文本可见
    text_marker.pose.orientation.w = 1.0;

    text_marker.scale.z = 0.16;//0.2  // 文本大小
    text_marker.color.a = a;//1.0
    text_marker.color.r = r;//1.0
    text_marker.color.g = g;//1.0
    text_marker.color.b = b;//1.0

    text_marker.text = datatxt;
    text_marker.lifetime = ros::Duration(lifetime); //0.5s后消失，输入0时为不消失
    Publish(pub_marker,text_marker); // 发布
}
//设置反光柱标记并发布 ros::Publisher  pub_all_marker_,pub_all_current_marker_;
//void  set_current_marker_property(double mx,double my,int n,std::string & frame_id,double & a,double & r,double & g,double &b,std::string &ns_id,double &timedate){
void  set_current_marker_property(const double mx,const double my,double width, double height,double length,int n,std::string & frame_id,double & a,double & r,double & g,double &b,std::string &ns_id,double &lifetime,ros::Publisher & pub_marker){

    visualization_msgs::Marker marker;
    /*决定从哪个视图可以看到标记r*/
    marker.header.frame_id = frame_id;//todo sukai 雷达的frame_id要改为laser_frame
    marker.ns = ns_id;
    marker.id = n;
    //设置标记类型：圆柱体
    marker.type = visualization_msgs::Marker::CYLINDER;

    //设置标记坐标
    marker.pose.position.x = mx;
    marker.pose.position.y = my;
    marker.pose.position.z = 0;

    //设置标记尺寸 0.09 0.09 0.50
    marker.scale.x = width;  //m
    marker.scale.y = height;
    marker.scale.z = length;

    //设置标记颜色
    marker.color.a = a; // Don't forget to set the alpha!
    marker.color.r = r;
    marker.color.g = g;
    marker.color.b = b;

    //设置标记动作
    marker.action = visualization_msgs::Marker::ADD;
    //timedate = 1.0 1s后消失
    marker.lifetime = ros::Duration(lifetime); //0.5s后消失，输入0时为不消失

    Publish(pub_marker,marker); // 发布
}

//苏凯 todo 设置反光板匹配标记并发布 ros::Publisher  pub_all_marker_,pub_all_current_marker_;
//void  set_current_light_panel_property(double lx, double ly, double width, double height, int n,std::string & frame_id,double & a,double & r,double & g,double &b,std::string &ns_id,double &timedate) {

void   set_current_light_panel_property(const double lx,const double ly, double width, double height,double length, int n,std::string & frame_id,double & a,double & r,double & g,double &b,std::string &ns_id,double &lifetime,ros::Publisher & pub_marker) {

    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;//todo sukai 雷达的frame_id要改为laser_frame

    //    决定从哪个视图可以看到光板
    marker.ns = ns_id;
    marker.id = n;
    // 设置标记类型：立方体（平面）
    marker.type = visualization_msgs::Marker::CUBE;

    // 设置标记坐标
    marker.pose.position.x = lx;
    marker.pose.position.y = ly;
    marker.pose.position.z = 0;

    // 设置标记尺寸
    marker.scale.x = width;  // 宽度
    marker.scale.y = height; // 高度
    marker.scale.z = length;  // 0.001厚度设为一个很小的值，表示一个平面

    // 设置标记颜色
    marker.color.a = a; // 透明度设为0.5，让光板半透明
    marker.color.r = r;
    marker.color.g = g;
    marker.color.b = b; // 黄色，你可以根据需要修改颜色

    // 设置标记动作
    marker.action = visualization_msgs::Marker::ADD;
    //timedate = 1.0 1s后消失
    marker.lifetime = ros::Duration(lifetime); // 1秒后消失，输入0时为不消失

    Publish(pub_marker,marker); // 发布
}

// 显示所有的反光柱,只显示一次
void fanguangbanShowAllMaker(const std::map<std::string ,std::shared_ptr<ReflectorPanelPoint>> &reflectorPanelPointAllMaps_) {
    std::cout << "--------------------------------显示所有的反光柱,只显示一次--------------------------------"<< std::endl;
    // 打印结果
    int show_all_marker_id = 0;
    for (const auto &entry : reflectorPanelPointAllMaps_) {
        // entry.first 是键（key），即std::string类型，代表反射板的标识符或名称
        const std::string &panelName = entry.first;
        // entry.second 是值（value），即std::shared_ptr<ReflectorPanelPoint>类型，指向反射板点的智能指针
        const std::shared_ptr<ReflectorPanelPoint> &panelPoint = entry.second;

        int reflectorPanelPoint_marker_id_=panelPoint->reflectorPanelPoint_marker_id_;
        int reflectorPanelPoint_text_id_=panelPoint->reflectorPanelPoint_text_id_;

        if(panelPoint->shape=="CYLINDER"){//CUBE：立方体（平面）,CYLINDER: 圆柱体
            //  std::cout <<" CYLINDER: 圆柱体" <<std::endl;
            //todo 发布标记 0.0 不消失
            // double a=0.7,r = 0.0,g = 0.0,b = 0.0;//黑色
            double a=0.6,r = 0.5,g = 0.0,b = 0.5;//紫色
            double a1=0.6,r1 = 0.5,g1 = 0.0,b1 = 0.5;

//             if(panelPoint->id!="ID1")
//                 continue;

            double timedata=0.0;
            //  std::this_thread::sleep_for(std::chrono::milliseconds(20));//todo 不延时会加载不到数据
            //  timedata=5.0;
            show_all_marker_id++;  // ros::Publisher  pub_all_marker_,pub_all_current_marker_;
            //  std::cout <<" CYLINDER: 圆柱体" <<"  x: "<<panelPoint->x<<"  ,y: " <<panelPoint->y<<" ,reflectorPanelPoint_marker_id_: "<<reflectorPanelPoint_marker_id_<<std::endl;
            set_current_marker_property(panelPoint->x,panelPoint->y,fangaungzhuWidth_,fangaungzhuHeight_,fangaungzhuLength_,reflectorPanelPoint_marker_id_,map_frame_id_,a, r ,g,b,panelPoint->id,timedata,pub_all_marker_);      //绘图  画反光柱标记
            show_all_marker_id++;//ros中的rviz显示标记id
            std::this_thread::sleep_for(std::chrono::milliseconds(10));//todo 不延时会加载不到数据
            double text_x=panelPoint->x+fangaungzhuWidth_,text_y= panelPoint->y+fangaungzhuWidth_;
            //  std::cout <<" 发布文字 CYLINDER: 圆柱体" <<"  x: "<<panelPoint->x<<"  ,y: " <<panelPoint->y<<" ,reflectorPanelPoint_text_id_: "<<reflectorPanelPoint_text_id_<<std::endl;
            //发布文字
            publishTextMarker(text_x, text_y,map_frame_id_, panelPoint->id,reflectorPanelPoint_text_id_, a1,r1,g1,b1,panelPoint->id,timedata,pub_all_marker_);
            std::this_thread::sleep_for(std::chrono::milliseconds(10));//todo 不延时会加载不到数据
        }else if(panelPoint->shape=="CUBE"){
            std::cout <<" CUBE: 立方体" <<std::endl;
            double a=0.6,r = 0.5,g = 0.0,b = 0.5;//紫色
            double a1=0.6,r1 = 0.5,g1 = 0.0,b1 = 0.5;
            double timedata=0.0;
            //timedata=5.0;
            //    std::this_thread::sleep_for(std::chrono::milliseconds(20));//todo 不延时会加载不到数据
            //todo 发布标记 0.0 不消失
            show_all_marker_id++;
            std::cout <<" CUBE: 立方体" <<"  x: "<<panelPoint->x<<"  ,y: " <<panelPoint->y<<" ,show_all_marker_id: "<<reflectorPanelPoint_marker_id_<<std::endl;
            set_current_light_panel_property(panelPoint->x,panelPoint->y,  fangaungbanWidth_,  fangaungbanHeight_,fangaungbanLength_, reflectorPanelPoint_marker_id_,map_frame_id_,a, r ,g,b,panelPoint->id,timedata,pub_all_marker_);
            show_all_marker_id++;//ros中的rviz显示标记id
            std::this_thread::sleep_for(std::chrono::milliseconds(10));//todo 不延时会加载不到数据
            double text_x=panelPoint->x+fangaungbanWidth_,text_y= panelPoint->y+fangaungbanWidth_;
            //发布文字
            publishTextMarker(text_x, text_y,map_frame_id_, panelPoint->id,reflectorPanelPoint_text_id_, a1,r1,g1,b1,panelPoint->id,timedata,pub_all_marker_);
            std::this_thread::sleep_for(std::chrono::milliseconds(10));//todo 不延时会加载不到数据
        }

        // 现在可以访问或操作panelPoint指向的对象了
        // 例如，打印反射板点的一些信息
        std::cout << "显示所有的反光柱 Panel Name: " << panelName << ", Point Info: ";
        if (panelPoint) { // 检查智能指针是否有效
            std::cout << panelPoint<< std::endl;
        } else {
            std::cout << "nullptr";
        }
        std::cout << std::endl;
    }

}
// 显示一个的反光柱,只显示一次
void fanguangbanShowOneMaker(std::shared_ptr<ReflectorPanelPoint> & reflectorPanelPoint) {
    std::cout << "--------"<< std::endl;

    const std::string &panelName = reflectorPanelPoint->id;
    // entry.second 是值（value），即std::shared_ptr<ReflectorPanelPoint>类型，指向反射板点的智能指针
    const std::shared_ptr<ReflectorPanelPoint> &panelPoint = reflectorPanelPoint;

    int reflectorPanelPoint_marker_id_=reflectorPanelPoint->reflectorPanelPoint_marker_id_;
    int reflectorPanelPoint_text_id_=reflectorPanelPoint->reflectorPanelPoint_text_id_;

    if(panelPoint->shape=="CYLINDER"){//CUBE：立方体（平面）,CYLINDER: 圆柱体
        //  std::cout <<" CYLINDER: 圆柱体" <<std::endl;
        //todo 发布标记 0.0 不消失
        // double a=0.7,r = 0.0,g = 0.0,b = 0.0;//黑色
        double a=0.6,r = 0.5,g = 0.0,b = 0.5;//紫色
        double a1=0.6,r1 = 0.5,g1 = 0.0,b1 = 0.5;

        double timedata=0.0;
        std::this_thread::sleep_for(std::chrono::milliseconds(50));//todo 不延时会加载不到数据
        //  timedata=5.0;
        // ros::Publisher  pub_all_marker_,pub_all_current_marker_;
        // std::cout <<" CYLINDER: 圆柱体" <<"  x: "<<panelPoint->x<<"  ,y: " <<panelPoint->y<<" ,show_all_marker_id: "<<reflectorPanelPoint_marker_id_<<std::endl;
        set_current_marker_property(panelPoint->x,panelPoint->y,fangaungzhuWidth_,fangaungzhuHeight_,fangaungzhuLength_,reflectorPanelPoint_marker_id_,map_frame_id_,a, r ,g,b,panelPoint->id,timedata,pub_all_marker_);      //绘图  画反光柱标记
        //ros中的rviz显示标记id
        std::this_thread::sleep_for(std::chrono::milliseconds(100));//todo 不延时会加载不到数据
        double text_x=panelPoint->x+fangaungzhuWidth_,text_y= panelPoint->y+fangaungzhuWidth_;
        //  std::cout <<" 发布文字 CYLINDER: 圆柱体" <<"  x: "<<panelPoint->x<<"  ,y: " <<panelPoint->y<<" ,show_all_marker_id: "<<reflectorPanelPoint_text_id_<<std::endl;
        //发布文字
        publishTextMarker(text_x, text_y,map_frame_id_, panelPoint->id,reflectorPanelPoint_text_id_, a1,r1,g1,b1,panelPoint->id,timedata,pub_all_marker_);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));//todo 不延时会加载不到数据
    }else if(panelPoint->shape=="CUBE"){
        std::cout <<" CUBE: 立方体" <<std::endl;
        double a=0.6,r = 0.5,g = 0.0,b = 0.5;//紫色
        double a1=0.6,r1 = 0.5,g1 = 0.0,b1 = 0.5;
        double timedata=0.0;
        //timedata=5.0;
        std::this_thread::sleep_for(std::chrono::milliseconds(100));//todo 不延时会加载不到数据
        //todo 发布标记 0.0 不消失

        std::cout <<" CUBE: 立方体" <<"  x: "<<panelPoint->x<<"  ,y: " <<panelPoint->y<<" ,show_all_marker_id: "<<reflectorPanelPoint_marker_id_<<std::endl;
        set_current_light_panel_property(panelPoint->x,panelPoint->y,  fangaungbanWidth_,  fangaungbanHeight_,fangaungbanLength_, reflectorPanelPoint_marker_id_,map_frame_id_,a, r ,g,b,panelPoint->id,timedata,pub_all_marker_);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));//todo 不延时会加载不到数据
        double text_x=panelPoint->x+fangaungbanWidth_,text_y= panelPoint->y+fangaungbanWidth_;
        //发布文字
        publishTextMarker(text_x, text_y,map_frame_id_, panelPoint->id,reflectorPanelPoint_text_id_, a1,r1,g1,b1,panelPoint->id,timedata,pub_all_marker_);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));//todo 不延时会加载不到数据
    }

    // 现在可以访问或操作panelPoint指向的对象了
    // 例如，打印反射板点的一些信息
    std::cout << "显示所有的反光柱 Panel Name: " << panelName << ", Point Info: ";
    if (panelPoint) { // 检查智能指针是否有效
        std::cout << panelPoint<< std::endl;
    } else {
        std::cout << "nullptr";
    }
    std::cout << std::endl;

}



//发布重定位
void publishInitialPose(const tf::Transform& mapTbaselinkTransform, const std::string& map_frame_id, ros::Publisher& initial_pose_pub) {


    // 记录 发布重定位 开始时间
    startTime_ = std::chrono::high_resolution_clock::now();
    // 构建PoseWithCovarianceStamped消息
    geometry_msgs::PoseWithCovarianceStamped initial_pose_msg;
    initial_pose_msg.header.stamp = ros::Time::now(); // 当前时间戳
    initial_pose_msg.header.frame_id = map_frame_id; // 父坐标系为map

    // 获取 transform 的平移部分
    const tf::Vector3& origin = mapTbaselinkTransform.getOrigin();
    initial_pose_msg.pose.pose.position.x = origin.x();
    initial_pose_msg.pose.pose.position.y = origin.y();
    initial_pose_msg.pose.pose.position.z = origin.z();

    // 获取 transform 的旋转部分
    const tf::Quaternion& quat = mapTbaselinkTransform.getRotation();
    initial_pose_msg.pose.pose.orientation.x = quat.x();
    initial_pose_msg.pose.pose.orientation.y = quat.y();
    initial_pose_msg.pose.pose.orientation.z = quat.z();
    initial_pose_msg.pose.pose.orientation.w = quat.w();

    // 设置协方差矩阵（这里为示例值，实际应用中应根据传感器精度调整）
    initial_pose_msg.pose.covariance = {
            1e-9, 0, 0, 0, 0, 0,
            0, 1e-3, 1e-9, 0, 0, 0,
            0, 0, 1e6, 0, 0, 0,
            0, 0, 0, 1e6, 0, 0,
            0, 0, 0, 0, 1e6, 0,
            0, 0, 0, 0, 0, 1e-9
    };

    // 发布消息
    initial_pose_pub.publish(initial_pose_msg);
}







//发布过滤后的只有强度数据的点云，雷达扫描到的反光柱轮廓
void convertAndPublish(const std::vector< Point >& points, ros::Publisher& pub_circle_pointCloud) {

//        while (pub_circle_pointCloud.getNumSubscribers() < 1) {//todo 判断订阅的对象是否创建
//        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
//        continue;
//    }
    std::cout<<"================发布过滤后的只有强度数据的点云，雷达扫描到的反光柱轮廓==============: "<<points.size()<<std::endl ;

    std::cout<<"===================convertAndPublish================: "<<points.size()<<std::endl ;

    sensor_msgs::PointCloud2 cloud_msg;
    cloud_msg.header.stamp = ros::Time::now();
    cloud_msg.header.frame_id = laser_frame_id_; // 根据需要设置frame_id

    // 设置点云消息的字段
    cloud_msg.height = 1;
    cloud_msg.width = points.size();
    cloud_msg.is_dense = false;

    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
    modifier.setPointCloud2FieldsByString(1, "xyz");
    modifier.resize(points.size());

    // 使用迭代器填充点云消息的数据
    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");

    for (const auto& point : points) {
        *iter_x = point.laserTscan_x;
        *iter_y = point.laserTscan_y;
        *iter_z = point.laserTscan_z;

        ++iter_x;
        ++iter_y;
        ++iter_z;
    }

    pub_circle_pointCloud.publish(cloud_msg);

    std::cout<<"end================发布过滤后的只有强度数据的点云，雷达扫描到的反光柱轮廓==============: "<<points.size()<<std::endl ;

}



//查找高强度点组作为反光柱并确定圆心，      //   circle = trigonometry(points2);   三角函数拟定圆心，根据公式原理编写用三角函数法拟定圆心的函数。4304
std::vector< Point > findCircle (const sensor_msgs::LaserScan &scan) {

    //重置
    if(show_marker_id_>10000)
        show_marker_id_=0;

    int show_marker_id=0;
    std::vector< Point > points;                          //储存全部高强点激光信息
    std::vector< Point > points2;                       //各个反光柱激光信息
    std::vector< Point > points3;                       //各个反观柱圆心坐标信息

    Point end{};
    end.scan_index=end_scan_index_;//扫描结束点 9999999
    end.range=0;
    end.intensity=0;

    end.x=0;
    end.y=0;
    end.yaw=0;
//    scan.ranges;
//    scan.intensities;
//    scan.angle_increment;

    // int HighStop = 1;  //从1号反光柱开始
    //每一个雷达点对应的强度数据【通过雷达点距离去 map 集合中找到对应的强度数据】
    double currentIntensitie=scan_intensitie_;//雷达到反光柱的强度，默认初始强度值，超出范围使用初始强度值

    for (int i = 0; i < scan.ranges.size(); i++) {        //获取高强度点

        //todo 1.判断每一个雷达点对应的强度数据【通过雷达点距离去map集合中找到对应的强度数据】
        {
            double currentRange = scan.ranges[i];//当前雷达到反光柱的距离
            //std::cout << "1.当前雷达到反光柱的距离:currentRange " <<currentRange<< std::endl;
            if (    std::isnan(currentRange) || std::isinf(currentRange)) {
                std::cout << "currentRange is a valid isnan or finite number." << std::endl;
                continue;
            }

            //从文件中读取scan_intensitie_key_ 强度值 ，以此从小到大判断强度值，scan_intensitie_key_的第一位数据是雷达距离反光板最短的数据。
            for(int j = 0; j < scan_intensitie_key_.size(); j++){ //scan_intensitie_key_ 储存的 int:雷达距离

                //获取当前距离值
                double currentDistance =  (double) scan_intensitie_key_[j];//当前索引   +range_threshole_
                //获取距离最大值
                double currentMaxDistance =  (double) scan_intensitie_key_[scan_intensitie_key_.size()-1] ;//当前索引

                double upDistance =0.0;//文件中储存的是int距离数据
                if(j>0){
                    upDistance= (double) scan_intensitie_key_[j-1];
                }
                //判断距离的范围
                if((currentRange>=upDistance && currentRange<=currentDistance)){//超出范围使用初始强度值，一般取最大距离对应的强度值
                    //通过当前scan_intensitie_key_距离值，取出scan_intensitie_map_中对应的强度值。
                    int key_to_check = scan_intensitie_key_[j]; // 用你要检查的键替换这个值


                    if (scan_intensitie_map_.find(key_to_check) != scan_intensitie_map_.end()) {
                        if(j>0){
                            if (scan_intensitie_map_.find(key_to_check-1) != scan_intensitie_map_.end()){
                                double   up_currentIntensitie = scan_intensitie_map_[key_to_check-1];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                                currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                                double difIntensitie_cm= (up_currentIntensitie-currentIntensitie)/100;//当前数据与之前数据的衰减值,衰减值除100cm，等于每厘米的衰减值
                                if(difIntensitie_cm<0){
                                    currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                                    continue;
                                }
                                double m=(double)(j-1);//当前满足多少m，并转成double
                                double currentRange_cm = (currentRange-m)*100;//减去整数 (double)j 后，得到0.3m，算成30cm.
                                double  intensitie_cm = currentRange_cm*difIntensitie_cm;//为当前的每厘米的衰减值
                                currentIntensitie = up_currentIntensitie-intensitie_cm;
//

                            }else{
                                currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                            }

                        }else{
                            currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                        }

                        //std::cout << "2.取出scan_intensitie_map_中对应的强度值。:int:雷达距离 " <<key_to_check<<" ,int:雷达强度 " <<currentIntensitie<<  std::endl;
                    } else {
                        std::cerr << "Key does not exist in the map." << std::endl;//区默认强度值
                    }
                }

                else if((j==scan_intensitie_key_.size()-1)&&currentRange>=currentMaxDistance){

                    //通过当前scan_intensitie_key_距离值，取出scan_intensitie_map_中对应的强度值。
                    int key_to_check = scan_intensitie_key_[scan_intensitie_key_.size()-1]; // 用你要检查的键替换这个值
                    if (scan_intensitie_map_.find(key_to_check) != scan_intensitie_map_.end()) {
                        currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                        // std::cout << "3.取出scan_intensitie_map_中对应的强度值。:int:雷达距离 " <<key_to_check<<" ,int:雷达强度 " <<currentIntensitie<<  std::endl;

                    } else {
                        std::cerr << "Key does not exist in the map." << std::endl;//区默认强度值
                    }

                }//if(currentRange>=currentMaxDistance)

            }// for(int j = 0; j < scan_intensitie_key_.size(); j++)

        }//end 判断每一个雷达点对应的强度数据【通过雷达点距离去map集合中找到对应的强度数据】

        //-------------------------------------------------------------------------
        Point high;
        high.scan_index = i;                                              //记录高强点数据
        high.range = scan.ranges[i] ;//+  fangaungzhuRadius_;    //查找最大值确定圆心方法需要加半径，三角函数法不需要
        high.intensity = scan.intensities[i];
        high.laserTscan_yaw = scan.angle_min + scan.angle_increment * high.scan_index;//yaw角度
        high.laserTscan_x = high.range * cos(high.laserTscan_yaw);
        high.laserTscan_y = high.range * sin(high.laserTscan_yaw);
        //std::cout << "1scan.intensities[i]: " << scan.intensities[i] << std::endl;
        // std::cout << "1high.intensity: " << high.intensity << std::endl;
        if (    std::isnan(high.range) || std::isinf(high.range)||
                std::isnan(high.laserTscan_x) || std::isinf(high.laserTscan_x)||
                std::isnan(high.laserTscan_y) || std::isinf(high.laserTscan_y)
                ) {
            std::cout << "range is a valid isnan or finite number." << std::endl;
            continue;
        }
//        tf::Quaternion q;
//        q.setRPY(0.0, 0.0, high.laserTscan_yaw);
        high.laserTscan_rx=0.0;
        high.laserTscan_ry=0.0;
        high.laserTscan_rz=0.0;
        high.laserTscan_rw=1.0;
        // todo 强度阙值，改到你的雷达实际返回的反光柱强度值。//  将trilateration.cpp中的第192行，
//        std::cout << "================================强度阙值，改到你的雷达实际返回的反光柱强度值===================================="<<std::endl;
//        std::cout << "high.range: " << std::to_string(high.range)<<std::endl;
//        std::cout << "scan.intensities[i]: " << std::to_string(scan.intensities[i])<<std::endl;
//        std::cout << "(currentIntensitie-intensitie_threshole_): " << (currentIntensitie-intensitie_threshole_)<<std::endl;

        if (scan.intensities[i] >= (currentIntensitie-intensitie_threshole_)){                    //强度阀值
            points.push_back(high);                          //将强度超过阀值的数据储存
        }else{//强度低于阀值时，判断这个点的上一个点与下一个点是否存在强度数据，满足强度数据就认为该点也满足强度数据
            int upindex =i-1;
            int dowindex =i+1;
            //超过索引边界就舍去
            if(upindex<0||(dowindex>scan.ranges.size()-1)){
                continue;
            }
            double up_intensitie  =      scan.intensities[upindex];
            double dow_intensitie  =     scan.intensities[dowindex];
            if(up_intensitie>=(currentIntensitie-intensitie_threshole_) && dow_intensitie>=(currentIntensitie-intensitie_threshole_) ){
                points.push_back(high);                          //将强度超过阀值的数据储存

            }// if(up_intensitie>scan_intensitie_&&dow_intensitie>scan_intensitie_)

        }//if (scan.intensities[i] > scan_intensitie_)
    }//for (int i = 0; i < scan.ranges.size(); i++)
//=============================================================================================================================
    //发布过滤后的只有强度数据的点云，雷达扫描到的反光柱轮廓
    if(isSend_publish_circle_pointCloud_)
        convertAndPublish(points, pub_circle_pointCloud_);

    //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
    if(points.size() >2){                                           //预防没有高强度数据输入发生报错

        int sum = points[0].scan_index - 1;
        points.push_back(end);                            //结束标志位，最后一个反光柱 指数 需要突变
        for (int j = 0; j< points.size(); j++) {          //分离各反光柱数据
            if((points[j].scan_index  - sum)== 1) {                //判断是否连续
                points2.push_back(points[j]);              //连续点为同一组
                // std::cout<<HighStop <<"号反光柱"<<std::endl;
                // std::cout<<"指数：" <<points[j].index  <<"，第"<<points2.size()<<"个，强度:"<<points[j].intensity<<std::endl;
            }else{
                //可以处理一组强度数据
                if(points2.size() > points_size_threshold_ ) {
                    // if(points2.size() > 2 ) {                                //至少连续三个点强度超过   强度阀值   即默认为反光柱,单独点滤除
                    // std::cout<<HighStop <<"号反光柱,数量：" <<points2.size()<<std::endl;
                    // circle = findMax(points2);                  //查找各个反光柱最大值确定圆心

                    Point circle;

                    //todo 三角函数拟定圆心，根据公式原理编写用三角函数法拟定圆心的函数。4304
                    circle = trigonometry(points2);            //三角函数拟定圆心
                    // circle = fitCircleCenter(points2);            //最小二乘法拟合圆心 todo 两者切换观察效果
                    circle.shape="CYLINDER";//CUBE：立方体（平面）,CYLINDER: 圆柱体
                    // circle.id=generate_shortened_string();// 简化的10位字符串生成器
                    //circle.id = "ID" + std::to_string(idCounter_++);
                    //std::cout<<"findCircle 反光柱中心点圆心点:"<<"距离:"<<circle.range<<"角度:"<<circle.laserTscan_yaw<<"x轴:"<<circle.laserTscan_x <<"y轴:"<<circle.laserTscan_y <<std::endl;
                    //4元素
//                        tf::Quaternion q;
//                        q.setRPY(0.0, 0.0, circle.laserTscan_yaw);
                    circle.laserTscan_rx=0.0;
                    circle.laserTscan_ry=0.0;
                    circle.laserTscan_rz=0.0;
                    circle.laserTscan_rw=1.0;


                    //todo 发布标记 0.1秒后消失
                    double a=0.5,r = 0.0,g = 0.0,b = 1.0;
                    double a1=0.5,r1 = 0.0,g1 = 0.0,b1 = 1.0;//double a1=0.5,r1 = 1.0,g1 = 1.0,b1 = 0.5;
                    double timedata=0.1;

                    show_marker_id++;  // // ros::Publisher  pub_all_marker_,pub_all_current_marker_;
                    //circle.id=generate_shortened_string();// 简化的10位字符串生成器

                    circle.id = "ID" + std::to_string(show_marker_id);
                    // std::this_thread::sleep_for(std::chrono::milliseconds(5));//todo 不延时会加载不到数据
                    // std::cout << "1.circle Position: (" << circle.laserTscan_x << ", " << circle.laserTscan_y << ", " << circle.laserTscan_z << "), intensity: " <<std::to_string(circle.intensity)<<" , range: " <<circle.range<< std::endl;

                    set_current_marker_property(circle.laserTscan_x,circle.laserTscan_y,fangaungzhuWidth_,fangaungzhuHeight_,fangaungzhuLength_,show_marker_id,laser_frame_id_,a, r ,g,b,circle.id,timedata,pub_all_current_marker_);      //绘图  画反光柱标记
                    // std::this_thread::sleep_for(std::chrono::milliseconds(5));//todo 不延时会加载不到数据
                    show_marker_id++;//ros中的rviz显示标记id
                    // std::cout << "2.circle Position: (" << circle.laserTscan_x << ", " << circle.laserTscan_y << ", " << circle.laserTscan_z << ") intensity: " <<circle.intensity<<" , range: " <<circle.range<< std::endl;

                    //发布文字
                    // publishTextMarker(circle.laserTscan_x, circle.laserTscan_y,laser_frame_id_, circle.id,show_marker_id, a1,r1,g1,b1,circle.id,timedata,pub_all_current_marker_);

                    points3.push_back(circle);                    //储存圆心点数据


                    // HighStop++;
                }
                points2.clear();
                points2.push_back(points[j]);

            }//if((points[j].scan_index  - sum)== 1)
            sum =points[j].scan_index;
        }//     for (int j = 0; j< points.size(); j++)
    }else{
        // std::cerr<<"无法找到有效数据" <<std::endl;
    }
    points.clear();
    return points3;
}

//-------------数据平滑------------------------
int wlsFilter_1d(vector<double> &vecInput, vector<double> & vecOutput, vector<double> &vecL, double lambda, double alpha, double smallNum)
{
    int number = vecInput.size();
    if (vecL.size() != number || number < 2) return -1;
    vector<double> vecDx(number + 1);
    vecDx[0] = 0;
    vecDx[number] = 0;
    for (int i = 1; i < number; i++)
    {
        vecDx[i] = lambda * (pow(abs(vecL[i] - vecL[i - 1]) + smallNum, alpha));
    }

    vector<Eigen::Triplet<double>> tripletList(3 * number - 2);
    for (int i = 0; i < number; i++)
    {
        tripletList[i] = T(i, i, 1 + vecDx[i] + vecDx[i + 1]);
    }
    for (int i = 1; i < number; i++)
    {
        tripletList[number + i - 1] = T(i, i - 1, -vecDx[i]);
    }
    for (int i = 1; i < number; i++)
    {
        tripletList[2 * number + i - 2] = T(i - 1, i, -vecDx[i]);
    }
    SpMat A(number, number);
    A.setFromTriplets(tripletList.begin(), tripletList.end());

    Eigen::VectorXd b(number);
    for (int i = 0; i < number; i++)
    {
        b(i) = vecInput[i];
    }
    // Solving:
    Eigen::SimplicialCholesky<SpMat> solver(A);
    //SimplicialLLT<SpMat> solver(A);
    Eigen::VectorXd x = solver.solve(b);
    for (int i = 0; i < number; i++)
    {
        vecOutput.push_back(x[i]);
    }
    return 0;
}


bool checkReflectorDiameter(const std::vector<Point> &points, double reflector_diameter) {
    // 检查是否有足够的点
    if (points.size() < 3) {
        return false;  // 点数量不足，无法判断
    }

    // 获取第一个点和最后一个点
    const Point &p1 = points.front();
    const Point &p2 = points.back();

    // 计算两个点之间的欧氏距离
    double distance = sqrt(pow(p2.laserTscan_x - p1.laserTscan_x, 2) + pow(p2.laserTscan_y - p1.laserTscan_y, 2));

    // 判断距离是否与反光柱的直径匹配（可以设置一个容差值，避免微小误差）
    double tolerance = 0.03;  // 例如，允许1厘米的误差 todo 设置在0.01就没数据了
    if (fabs(distance - reflector_diameter) <= tolerance) {
        return true;  // 距离匹配，反光柱直径正确
    } else {
        return false;  // 距离不匹配
    }
}



//12926 scan_CB_new1 查找高强度点组作为反光柱并确定圆心，      //   circle = trigonometry(points2);   三角函数拟定圆心，根据公式原理编写用三角函数法拟定圆心的函数。4304
std::vector< Point > findCircle (const sensor_msgs::LaserScan &scan,const tf::StampedTransform &odomTlaser_transform_1,const tf::StampedTransform& transform_map_laser_1,const tf::StampedTransform &odomTbase_link_transform_1) {

    //重置
    if(show_marker_id_>10000)
        show_marker_id_=0;



    //---------end-------数据平滑 算法--------------------------------

    int show_marker_id=0;
    std::vector< Point > points;                          //储存全部高强点激光信息
    std::vector< Point > points2;                       //各个反光柱激光信息
    std::vector< Point > points3;                       //各个反观柱圆心坐标信息

    Point end{};
    end.scan_index=end_scan_index_;//扫描结束点 9999999
    end.range=0;
    end.intensity=0;

    end.x=0;
    end.y=0;
    end.yaw=0;
//    scan.ranges;
//    scan.intensities;
//    scan.angle_increment;

    // int HighStop = 1;  //从1号反光柱开始
    //每一个雷达点对应的强度数据【通过雷达点距离去 map 集合中找到对应的强度数据】
    double currentIntensitie=scan_intensitie_;//雷达到反光柱的强度，默认初始强度值，超出范围使用初始强度值

    for (int i = 0; i < scan.ranges.size(); i++) {        //获取高强度点

        //todo 1.判断每一个雷达点对应的强度数据【通过雷达点距离去map集合中找到对应的强度数据】
        {
            double currentRange = scan.ranges[i];//当前雷达到反光柱的距离
            //std::cout << "1.当前雷达到反光柱的距离:currentRange " <<currentRange<< std::endl;
            if (    std::isnan(currentRange) || std::isinf(currentRange)) {
                std::cout << "currentRange is a valid isnan or finite number." << std::endl;
                continue;
            }

            //从文件中读取scan_intensitie_key_ 强度值 ，以此从小到大判断强度值，scan_intensitie_key_的第一位数据是雷达距离反光板最短的数据。
            for(int j = 0; j < scan_intensitie_key_.size(); j++){ //scan_intensitie_key_ 储存的 int:雷达距离

                //获取当前距离值
                double currentDistance =  (double) scan_intensitie_key_[j];//当前索引   +range_threshole_
                //获取距离最大值
                double currentMaxDistance =  (double) scan_intensitie_key_[scan_intensitie_key_.size()-1] ;//当前索引

                double upDistance =0.0;//文件中储存的是int距离数据
                if(j>0){
                    upDistance= (double) scan_intensitie_key_[j-1];
                }
                //判断距离的范围
                if((currentRange>=upDistance && currentRange<=currentDistance)){//超出范围使用初始强度值，一般取最大距离对应的强度值
                    //通过当前scan_intensitie_key_距离值，取出scan_intensitie_map_中对应的强度值。
                    int key_to_check = scan_intensitie_key_[j]; // 用你要检查的键替换这个值


                    if (scan_intensitie_map_.find(key_to_check) != scan_intensitie_map_.end()) {
                        if(j>0){
                            if (scan_intensitie_map_.find(key_to_check-1) != scan_intensitie_map_.end()){
                                double   up_currentIntensitie = scan_intensitie_map_[key_to_check-1];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                                currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                                double difIntensitie_cm= (up_currentIntensitie-currentIntensitie)/100;//当前数据与之前数据的衰减值,衰减值除100cm，等于每厘米的衰减值
                                if(difIntensitie_cm<0){
                                    currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                                    continue;
                                }
                                double m=(double)(j-1);//当前满足多少m，并转成double
                                double currentRange_cm = (currentRange-m)*100;//减去整数 (double)j 后，得到0.3m，算成30cm.
                                double  intensitie_cm = currentRange_cm*difIntensitie_cm;//为当前的每厘米的衰减值
                                currentIntensitie = up_currentIntensitie-intensitie_cm;
//

                            }else{
                                currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                            }

                        }else{
                            currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                        }

                        //std::cout << "2.取出scan_intensitie_map_中对应的强度值。:int:雷达距离 " <<key_to_check<<" ,int:雷达强度 " <<currentIntensitie<<  std::endl;
                    } else {
                        std::cerr << "Key does not exist in the map." << std::endl;//区默认强度值
                    }
                }

                else if((j==scan_intensitie_key_.size()-1)&&currentRange>=currentMaxDistance){

                    //通过当前scan_intensitie_key_距离值，取出scan_intensitie_map_中对应的强度值。
                    int key_to_check = scan_intensitie_key_[scan_intensitie_key_.size()-1]; // 用你要检查的键替换这个值
                    if (scan_intensitie_map_.find(key_to_check) != scan_intensitie_map_.end()) {
                        currentIntensitie=scan_intensitie_map_[key_to_check];//scan_intensitie_map_=》 int:雷达距离 value:雷达强度
                        // std::cout << "3.取出scan_intensitie_map_中对应的强度值。:int:雷达距离 " <<key_to_check<<" ,int:雷达强度 " <<currentIntensitie<<  std::endl;

                    } else {
                        std::cerr << "Key does not exist in the map." << std::endl;//区默认强度值
                    }

                }//if(currentRange>=currentMaxDistance)

            }// for(int j = 0; j < scan_intensitie_key_.size(); j++)

        }//end 判断每一个雷达点对应的强度数据【通过雷达点距离去map集合中找到对应的强度数据】

        //-------------------------------------------------------------------------
        Point high;
        high.scan_index = i;                                              //记录高强点数据
        high.range = scan.ranges[i] ;//+  fangaungzhuRadius_;    //查找最大值确定圆心方法需要加半径，三角函数法不需要
        high.intensity = scan.intensities[i];
        high.laserTscan_yaw = scan.angle_min + scan.angle_increment * high.scan_index;//yaw角度
        high.laserTscan_x = high.range * cos(high.laserTscan_yaw);
        high.laserTscan_y = high.range * sin(high.laserTscan_yaw);
        //std::cout << "1scan.intensities[i]: " << scan.intensities[i] << std::endl;
        // std::cout << "1high.intensity: " << high.intensity << std::endl;
        if (    std::isnan(high.range) || std::isinf(high.range)||
                std::isnan(high.laserTscan_x) || std::isinf(high.laserTscan_x)||
                std::isnan(high.laserTscan_y) || std::isinf(high.laserTscan_y)
                ) {
            std::cout << "range is a valid isnan or finite number." << std::endl;
            continue;
        }
//        tf::Quaternion q;
//        q.setRPY(0.0, 0.0, high.laserTscan_yaw);
        high.laserTscan_rx=0.0;
        high.laserTscan_ry=0.0;
        high.laserTscan_rz=0.0;
        high.laserTscan_rw=1.0;
        // todo 强度阙值，改到你的雷达实际返回的反光柱强度值。//  将trilateration.cpp中的第192行，
//        std::cout << "================================强度阙值，改到你的雷达实际返回的反光柱强度值===================================="<<std::endl;
//        std::cout << "high.range: " << std::to_string(high.range)<<std::endl;
//        std::cout << "scan.intensities[i]: " << std::to_string(scan.intensities[i])<<std::endl;
//        std::cout << "(currentIntensitie-intensitie_threshole_): " << (currentIntensitie-intensitie_threshole_)<<std::endl;

        if (scan.intensities[i] >= (currentIntensitie-intensitie_threshole_)){                    //强度阀值
            points.push_back(high);                          //将强度超过阀值的数据储存
        }else{//强度低于阀值时，判断这个点的上一个点与下一个点是否存在强度数据，满足强度数据就认为该点也满足强度数据
            int upindex =i-1;
            int dowindex =i+1;
            //超过索引边界就舍去
            if(upindex<0||(dowindex>scan.ranges.size()-1)){
                continue;
            }
            double up_intensitie  =      scan.intensities[upindex];
            double dow_intensitie  =     scan.intensities[dowindex];
            if(up_intensitie>=(currentIntensitie-intensitie_threshole_) && dow_intensitie>=(currentIntensitie-intensitie_threshole_) ){
                points.push_back(high);                          //将强度超过阀值的数据储存

            }// if(up_intensitie>scan_intensitie_&&dow_intensitie>scan_intensitie_)

        }//if (scan.intensities[i] > scan_intensitie_)
    }//for (int i = 0; i < scan.ranges.size(); i++)
//=============================================================================================================================
    //发布过滤后的只有强度数据的点云，雷达扫描到的反光柱轮廓
    if(isSend_publish_circle_pointCloud_)
        convertAndPublish(points, pub_circle_pointCloud_);

    //----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
    if(points.size() >2){                                           //预防没有高强度数据输入发生报错

        int sum = points[0].scan_index - 1;
        points.push_back(end);                            //结束标志位，最后一个反光柱 指数 需要突变
        for (int j = 0; j< points.size(); j++) {          //分离各反光柱数据
            if((points[j].scan_index  - sum)== 1) {                //判断是否连续
                points2.push_back(points[j]);              //连续点为同一组
                // std::cout<<HighStop <<"号反光柱"<<std::endl;
                // std::cout<<"指数：" <<points[j].index  <<"，第"<<points2.size()<<"个，强度:"<<points[j].intensity<<std::endl;
            }else{
                //可以处理一组强度数据
                if(points2.size() > points_size_threshold_ ) {
                    // if(points2.size() > 2 ) {                                //至少连续三个点强度超过   强度阀值   即默认为反光柱,单独点滤除
                    // std::cout<<HighStop <<"号反光柱,数量：" <<points2.size()<<std::endl;
                    // circle = findMax(points2);                  //查找各个反光柱最大值确定圆心




                    // 先对数据进行平滑处理
                    //  std::vector<Point> smoothed_points = movingAverageFilter(points2, window_size_);
                    //计算雷达扫描到的反光柱2端的距离是否满足反光柱直径
                    bool isReflectorMatched = checkReflectorDiameter(points2, fangaungzhuWidth_);
                    if(!isReflectorMatched)
                        continue;


                    //如果你想每隔 2 个点采样一次，并使用窗口大小为 5 的移动平均滤波，可以调用：
                    std::vector<Point> smoothed_points = movingAverageFilter(points2, window_size_, 2);
                    // std::vector<Point> smoothed_points1 = movingAverageFilter(smoothed_points, window_size_, 3);
                    //发布过滤后的只有强度数据的点云，雷达扫描到的反光柱轮廓 "/trilateration_circle_points"
                    //if(isSend_publish_circle_pointCloud_)
                    convertAndPublish(smoothed_points, pub_circle_pointCloud_);



                    Point circle;

                    //todo 三角函数拟定圆心，根据公式原理编写用三角函数法拟定圆心的函数。4304
                    // circle = trigonometry(points2);            //三角函数拟定圆心




                    //反光板宽度
//                    double reflector_width=fangaungbanWidth_;
//                    反光柱半径
//                    double radius_offset=fangaungzhuRadius_;
                    //  circle = trigonometryForReflector(points2,fangaungbanWidth_,fangaungzhuRadius_);
                    //  circle =trigonometryForReflectorNew(points2, fangaungbanWidth_, fangaungzhuRadius_) ;
                    circle =trigonometryForReflectorNew(smoothed_points, fangaungbanWidth_, fangaungzhuRadius_) ;


                    // 反光柱不用三角函数拟合圆心，也用 【反光片，进行直线计算并使用两端点计算中心。】
                    //circle = calculateForBoard(points2, fangaungbanWidth_, fangaungzhuRadius_);

                    //完全不对
                    // circle = trigonometryForReflector(points2,fangaungbanWidth_,fangaungzhuRadius_,odomTlaser_transform_1);



                    //效果不好
                    //              circle =computeCircleCenterWithRadiusAndAngleCompensation(points2); //最小二乘法拟合圆心
//
//                        //circle = fitCircleCenter(points2);            //最小二乘法拟合圆心



                    // circle = fitCircleCenter(points2);            //最小二乘法拟合圆心 todo 两者切换观察效果
                    circle.shape="CYLINDER";//CUBE：立方体（平面）,CYLINDER: 圆柱体
                    // circle.id=generate_shortened_string();// 简化的10位字符串生成器
                    //circle.id = "ID" + std::to_string(idCounter_++);
                    //std::cout<<"findCircle 反光柱中心点圆心点:"<<"距离:"<<circle.range<<"角度:"<<circle.laserTscan_yaw<<"x轴:"<<circle.laserTscan_x <<"y轴:"<<circle.laserTscan_y <<std::endl;
                    //4元素
//                        tf::Quaternion q;
//                        q.setRPY(0.0, 0.0, circle.laserTscan_yaw);
                    circle.laserTscan_rx=0.0;
                    circle.laserTscan_ry=0.0;
                    circle.laserTscan_rz=0.0;
                    circle.laserTscan_rw=1.0;

                    // 使用姿态 yaw 创建变换 laser
                    tf::Transform scanPoint_to_laser_transform;
                    scanPoint_to_laser_transform.setOrigin(tf::Vector3(circle.laserTscan_x, circle.laserTscan_y, 0.0));
                    tf::Quaternion q;
                    q.setRPY(0, 0, circle.laserTscan_yaw);
                    scanPoint_to_laser_transform.setRotation(q);
                    tf::Transform scanPoint_to_laser_transform1 = scanPoint_to_laser_transform;
                    //----------------------------------------
                    tf::Transform scanPoint_to_odom_transform = odomTlaser_transform_1 * scanPoint_to_laser_transform;
                    //雷达数据从雷达坐标系转换到odom坐标系
                    circle.odomTscan_x = scanPoint_to_odom_transform.getOrigin().x();
                    circle.odomTscan_y = scanPoint_to_odom_transform.getOrigin().y();
                    circle.odomTscan_z = scanPoint_to_odom_transform.getOrigin().z();
                    circle.odomTscan_yaw = tf::getYaw(scanPoint_to_odom_transform.getRotation());
                    circle.odomTscan_rx=scanPoint_to_odom_transform.getRotation().x();
                    circle.odomTscan_ry=scanPoint_to_odom_transform.getRotation().y();
                    circle.odomTscan_rz=scanPoint_to_odom_transform.getRotation().z();
                    circle.odomTscan_rw=scanPoint_to_odom_transform.getRotation().w();

                    circle.odomTlaser_transform_1=odomTlaser_transform_1;
                    //---------------------------------------

                    tf::Transform scanPoint_to_map_transform = transform_map_laser_1 * scanPoint_to_laser_transform1;
                    //雷达数据从雷达坐标系转换到地图坐标系  mapTscanPoint  =  mapTlaser* laserTscanPoint
                    circle.x = scanPoint_to_map_transform.getOrigin().x();
                    circle.y = scanPoint_to_map_transform.getOrigin().y();
                    circle.z = scanPoint_to_map_transform.getOrigin().z();
                    circle.yaw = tf::getYaw(scanPoint_to_map_transform.getRotation());
                    circle.rx=scanPoint_to_map_transform.getRotation().x();
                    circle.ry=scanPoint_to_map_transform.getRotation().y();
                    circle.rz=scanPoint_to_map_transform.getRotation().z();
                    circle.rw=scanPoint_to_map_transform.getRotation().w();

                    circle.transform_map_laser_1=transform_map_laser_1;
                    circle.odomTbase_link_transform_1=odomTbase_link_transform_1;
                    //----------------------------------------

                    //todo 发布标记 0.1秒后消失
                    double a=1.0,r = 0.0,g = 0.0,b = 1.0;
                    double a1=0.5,r1 = 0.0,g1 = 0.0,b1 = 1.0;//double a1=0.5,r1 = 1.0,g1 = 1.0,b1 = 0.5;
                    double timedata=0.1;

                    show_marker_id++;  // // ros::Publisher  pub_all_marker_,pub_all_current_marker_;
                    //circle.id=generate_shortened_string();// 简化的10位字符串生成器

                    circle.id = "ID" + std::to_string(show_marker_id);
                    // std::this_thread::sleep_for(std::chrono::milliseconds(5));//todo 不延时会加载不到数据
                    // std::cout << "1.circle Position: (" << circle.laserTscan_x << ", " << circle.laserTscan_y << ", " << circle.laserTscan_z << "), intensity: " <<std::to_string(circle.intensity)<<" , range: " <<circle.range<< std::endl;

                    set_current_marker_property(circle.laserTscan_x,circle.laserTscan_y,fangaungzhuWidth_,fangaungzhuHeight_,fangaungzhuLength_,show_marker_id,laser_frame_id_,a, r ,g,b,circle.id,timedata,pub_all_current_marker_);      //绘图  画反光柱标记
                    // std::this_thread::sleep_for(std::chrono::milliseconds(5));//todo 不延时会加载不到数据
                    show_marker_id++;//ros中的rviz显示标记id
                    // std::cout << "2.circle Position: (" << circle.laserTscan_x << ", " << circle.laserTscan_y << ", " << circle.laserTscan_z << ") intensity: " <<circle.intensity<<" , range: " <<circle.range<< std::endl;

                    //发布文字
                    // publishTextMarker(circle.laserTscan_x, circle.laserTscan_y,laser_frame_id_, circle.id,show_marker_id, a1,r1,g1,b1,circle.id,timedata,pub_all_current_marker_);

                    points3.push_back(circle);                    //储存圆心点数据


                    // HighStop++;
                }
                points2.clear();
                points2.push_back(points[j]);

            }//if((points[j].scan_index  - sum)== 1)
            sum =points[j].scan_index;
        }//     for (int j = 0; j< points.size(); j++)
    }else{
        // std::cerr<<"无法找到有效数据" <<std::endl;
    }
    points.clear();
    return points3;
}




// 初始 反光板的数据，ReflectorPanelPoint 对象管理
void  readCircledateEncapsulationToMapAndvector(std::vector<Point> &points,int idCounter){
    //  读取 points 数据
    //  int idCounter = 1; //生成id用的
    int marker_id = 0;
    for (const Point& point : points) {
        std::string id = "ID" + std::to_string(idCounter++);
        // std::string id = "ID" + generate_shortened_string();
        // 使用 ReflectorPanelPoint 的构造函数来创建实例
        //  auto newPoint = ReflectorPanelPoint::createInstance1(id);
        auto  newPoint = ReflectorPanelPoint::createInstance2(id,marker_id, point.x, point.y, 0.0,0.0,0.0,0.0,1.0);

//        string newPoint_shape = "CYLINDER";//储存反光板的形状
//        int  marker_id_1 = marker_id+1;
//        auto  newPoint3 = ReflectorPanelPoint::createInstance3(id,marker_id, point.x, point.y, 0.0,0.0,0.0,0.0,1.0,newPoint_shape,marker_id_1);

        newPoint->shape="CYLINDER";//储存反光板的形状
        //newPoint->reflectorPanelPoint_marker_id_=marker_id; //marker_id 已经加入了
        newPoint->reflectorPanelPoint_text_id_=marker_id+1;
        //key为反光板id
        reflectorPanelPointAllMaps_[newPoint->id]=newPoint;//map集合储存所有点位，
        // std::vector 储存所有 排序好的反光板数据 自动排序
        //reflectorPanelPoints_vector_.insert(newPoint);//列表储存所有点位，通过tf订阅数据，实现自动排序功能
        addInsertNewPointToReflectorPanelPoints_vector(reflectorPanelPoints_vector_,newPoint);

        // 储存所有的点为 std::tuple<double, double> 为 key
        reflectorPanelPointAllUnordered_maps_[std::make_tuple(point.x, point.y)]=newPoint;
        marker_id=marker_id+2;
        //打印数据
        std::cout<<newPoint<<std::endl;
    }

}
//数据从雷达坐标系转换到地图坐标系    mapTlaser* laserTscanPoint
std::vector<Point> transformToMapFrame(const std::vector<Point>& points, const tf::Transform& transform_map_laser) {
    std::vector<Point> transformed_points;
    /**
   //todo 未加入姿态角度
    for (const auto& point : points) {
        tf::Vector3 scanPoint_to_laser(point.laserTscan_x, point.laserTscan_y, 0.0);
        tf::Vector3 p_transformed = transform_map_scanPoint * scanPoint_to_laser;
        Point transformed_point;
        transformed_point.laserTscan_x = p_transformed.x();
        transformed_point.laserTscan_y = p_transformed.y();
        transformed_points.push_back(transformed_point);
    }
    */




    //todo 加入当前扫描到的姿态角度
    for (const auto& point : points) {

        Point transformed_point;

        transformed_point.scan_index = point.scan_index;
        transformed_point.range = point.range;
        transformed_point.intensity = point.intensity;
        transformed_point.shape = point.shape;
        transformed_point.id = point.id;

        //雷达在激光雷达坐标系下的坐标 laserTscanPoint
        transformed_point.laserTscan_x = point.laserTscan_x;
        transformed_point.laserTscan_y = point.laserTscan_y;
        transformed_point.laserTscan_z = point.laserTscan_z;
        transformed_point.laserTscan_yaw = point.laserTscan_yaw;
        transformed_point.laserTscan_rx=point.laserTscan_rx;
        transformed_point.laserTscan_ry=point.laserTscan_ry;
        transformed_point.laserTscan_rz=point.laserTscan_rz;
        transformed_point.laserTscan_rw=point.laserTscan_rw;

        ROS_INFO_STREAM("数据从雷达坐标系转换到地图坐标系 Point " << ":(" << point.laserTscan_x << ", " << point.laserTscan_y << ")");


        // 使用姿态 yaw 创建变换 laser
        tf::Transform scanPoint_to_laser_transform;
        scanPoint_to_laser_transform.setOrigin(tf::Vector3(point.laserTscan_x, point.laserTscan_y, 0.0));
        tf::Quaternion q;
        q.setRPY(0, 0, point.laserTscan_yaw);
        scanPoint_to_laser_transform.setRotation(q);
        // 计算 scanPoint 雷达点云 在 map 坐标系下的变换
        // tf::Transform map_to_odom_transform = base_link_to_map_transform * odom_to_base_link_transform.inverse();
        tf::Transform scanPoint_to_map_transform = transform_map_laser * scanPoint_to_laser_transform;


        //雷达数据从雷达坐标系转换到地图坐标系  mapTscanPoint  =  mapTlaser* laserTscanPoint
        transformed_point.x = scanPoint_to_map_transform.getOrigin().x();
        transformed_point.y = scanPoint_to_map_transform.getOrigin().y();
        transformed_point.z = scanPoint_to_map_transform.getOrigin().z();
        transformed_point.yaw = tf::getYaw(scanPoint_to_map_transform.getRotation());
        transformed_point.rx=scanPoint_to_map_transform.getRotation().x();
        transformed_point.ry=scanPoint_to_map_transform.getRotation().y();
        transformed_point.rz=scanPoint_to_map_transform.getRotation().z();
        transformed_point.rw=scanPoint_to_map_transform.getRotation().w();
        transformed_points.push_back(transformed_point);

    }






    return transformed_points;
}



//基于雷达坐标系的数据， circle数据转换成map坐标系的数据
std::vector<Point> transformToMapFrame(const std::vector<Point>& points, double robot_x, double robot_y, double robot_yaw) {
    std::vector<Point> transformed_points;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(robot_x, robot_y, 0.0));
    tf::Quaternion q;
    q.setRPY(0.0, 0.0, robot_yaw);
    transform.setRotation(q);

    for (const auto& point : points) {
        tf::Vector3 p(point.laserTscan_x, point.laserTscan_y, 0.0);
        tf::Vector3 p_transformed = transform * p;
        Point transformed_point;
        transformed_point.laserTscan_x = p_transformed.x();
        transformed_point.laserTscan_y = p_transformed.y();
        transformed_points.push_back(transformed_point);
    }
    return transformed_points;
}


//超过时间阀值，就认为反光板已经失去匹配关系。删除失去匹配关系的数据
void removeStaleReflectors() {
    if(iswirtMatchokReflectoUnordered_map_){
        return;}
    auto now = ros::Time::now();



//    for (auto it = reflectorPanelPointAllMaps_.begin(); it != reflectorPanelPointAllMaps_.end();) {
//        if ((now - it->second->last_seen_).toSec() > matchok_timeout_) {
//            if(it->second->isMatchok_==1){
//                it->second->isMatchok_=0;//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
//                it->second->currentIsMatchok_=0;//-1：未开始匹配  0： laserTscan_x 已经被使用了，laserTscan_x这数据需只能通过tf或odom的变化量来预测更新成为最新的数据  1:匹配上并给雷达 laserTscan_x 数据赋值了
//            }
//        } else {
//            ++it;
//        }
//    }

    for (auto it = matchokReflectorPanelPointEnd_Unordered_map_.begin(); it != matchokReflectorPanelPointEnd_Unordered_map_.end();) {
        if ((now - it->second->last_seen_).toSec() > matchok_timeout_) {
            if(it->second->isMatchok_==1){
                it->second->isMatchok_=0;//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
                it->second->currentIsMatchok_=0;//-1：未开始匹配  0： laserTscan_x 已经被使用了，laserTscan_x这数据需只能通过tf或odom的变化量来预测更新成为最新的数据  1:匹配上并给雷达 laserTscan_x 数据赋值了
            }

            it = matchokReflectorPanelPointEnd_Unordered_map_.erase(it);
        } else {
            ++it;
        }
    }
}


//到所有反光板 flectorPanelPoints_vector 数据中去匹配;matchokReflectorPanelPointEnd_Unordered_map_.push_back
bool findPointByreflectorPanelPoints_vector(int &i,const std::vector<Point> &transformed_circle,tf::StampedTransform &laser_to_odom_transform,std::vector<std::shared_ptr<ReflectorPanelPoint>> & indices_first,std::shared_ptr<ReflectorPanelPoint> &foundPoint_result) {
    iswirtortSetPeriodically_=true;
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    // 添加一个函数来查找满足误差范围的点,reflectorPanelPoints_vector_size_=1代表全部遍历，2代表只遍历前1/2的数据，3代表只遍历前1/3的数据;具体评估雷达可见范围内可预测多好数据
    foundPoint_result = findPointByHalf(reflectorPanelPoints_vector_, transformed_circle[i].x, transformed_circle[i].y,
                                        epsilon_);
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    iswirtortSetPeriodically_=false;

    if (foundPoint_result) {
        foundPoint_result->isAssignment_ = -1;//可以给laserTscan_x赋值
        foundPoint_result->last_seen_ = ros::Time::now();
        foundPoint_result->isMatchok_ = 1;
        foundPoint_result->currentIsMatchok_ = 1;
        // laserTscanPoint
        foundPoint_result->laserTscan_x = transformed_circle[i].laserTscan_x;
        foundPoint_result->laserTscan_y = transformed_circle[i].laserTscan_y;
        foundPoint_result->laserTscan_z = transformed_circle[i].laserTscan_z;
        foundPoint_result->laserTscan_yaw = transformed_circle[i].laserTscan_yaw;
        foundPoint_result->laserTscan_rx = transformed_circle[i].laserTscan_rx;
        foundPoint_result->laserTscan_ry = transformed_circle[i].laserTscan_ry;
        foundPoint_result->laserTscan_rz = transformed_circle[i].laserTscan_rz;
        foundPoint_result->laserTscan_rw = transformed_circle[i].laserTscan_rw;


        foundPoint_result->scan_index = transformed_circle[i].scan_index;
        foundPoint_result->range = transformed_circle[i].range;
        foundPoint_result->intensity = transformed_circle[i].intensity;

        foundPoint_result->last_laser_to_odom_transform_ = laser_to_odom_transform;

        foundPoint_result->isAssignment_ = 1;
        indices_first.push_back(foundPoint_result);
        //加入已经匹配上的反光板,禁止查询
        iswirtMatchokReflectoUnordered_map_=true;
        std::this_thread::sleep_for(std::chrono::milliseconds(2));
        matchokReflectorPanelPointEnd_Unordered_map_[std::make_tuple(foundPoint_result->x, foundPoint_result->y)] = foundPoint_result;
        std::this_thread::sleep_for(std::chrono::milliseconds(2));
        iswirtMatchokReflectoUnordered_map_=false;
        std::cout << "Found point at (" << foundPoint_result->x << ", " << foundPoint_result->y << ")" << std::endl;
        return true;
    } else {

        return false;
    }
}
//到所有反光板 ReflectorPanelPointAllUnordered_maps 数据中去匹配,matchokReflectorPanelPointEnd_Unordered_map_.push_back
bool findPointByReflectorPanelPointAllUnordered_maps(int &i,const std::vector<Point> &transformed_circle,tf::StampedTransform &laser_to_odom_transform,std::vector<std::shared_ptr<ReflectorPanelPoint>> & indices_first,std::shared_ptr<ReflectorPanelPoint> &foundPoint_result){
    //到所有反光板数据中去匹配  auto
    auto it = reflectorPanelPointAllUnordered_maps_.find(std::make_tuple(transformed_circle[i].x, transformed_circle[i].y));
    if (it != reflectorPanelPointAllUnordered_maps_.end()) {
        if(it->second->isMatchok_==2){//非预测的数据直接不要 ，-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测到下一时刻的需匹配反光板
            std::cout << "Found reflectorPanelPointAllUnordered_maps_ =>  ReflectorPanelPoint: (" << it->second->x << ", " << it->second->y << "), ID: " << it->second->id << std::endl;
            it->second->isAssignment_=-1;//可以给laserTscan_x赋值
            // laserTscanPoint
            it->second->laserTscan_x=transformed_circle[i].laserTscan_x;
            it->second->laserTscan_y=transformed_circle[i].laserTscan_y;
            it->second->laserTscan_z=transformed_circle[i].laserTscan_z;
            it->second->laserTscan_yaw=transformed_circle[i].laserTscan_yaw;

            it->second->laserTscan_rx=transformed_circle[i].laserTscan_rx;
            it->second->laserTscan_ry=transformed_circle[i].laserTscan_ry;
            it->second->laserTscan_rz=transformed_circle[i].laserTscan_rz;
            it->second->laserTscan_rw=transformed_circle[i].laserTscan_rw;

            it->second->scan_index = transformed_circle[i].scan_index;
            it->second->range = transformed_circle[i].range;
            it->second->intensity = transformed_circle[i].intensity;

            //记录当前里程计数据
            it->second->last_laser_to_odom_transform_=laser_to_odom_transform;
            it->second->last_seen_= ros::Time::now();//当前匹配上的时间
            it->second->isMatchok_=1;//表示匹配上了 -1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
            it->second->currentIsMatchok_ = 1;
            it->second->isAssignment_=1;//赋值结束，不可以给laserTscan_x赋值
            indices_first.push_back(it->second);
            foundPoint_result=it->second;
            //加入已经匹配上的反光板,禁止查询
            iswirtMatchokReflectoUnordered_map_=true;
            std::this_thread::sleep_for(std::chrono::milliseconds(2));
            //加入已经匹配上的反光板
            matchokReflectorPanelPointEnd_Unordered_map_[std::make_tuple(it->second->x, it->second->y)]=it->second;
            std::this_thread::sleep_for(std::chrono::milliseconds(2));
            iswirtMatchokReflectoUnordered_map_=false;

            return true;
        } else { // if(it->second->isMatchok_==2)
            return false;
        }
    } else {
        return false;
    }
}


// 计算两个点之间的欧氏距离
double euclideanDistance(const Point& p1, const Point& p2) {
    return std::sqrt(std::pow(p1.laserTscan_x - p2.laserTscan_x, 2) + std::pow(p1.laserTscan_y - p2.laserTscan_y, 2));
}

// 过滤掉两俩之间距离太近的点
std::vector<Point> filterPoints(const std::vector<Point>& points, double threshold) {
    std::vector<Point> filteredPoints;
    std::vector<bool> valid(points.size(), true);

    for (size_t i = 0; i < points.size(); ++i) {
        if (!valid[i]) continue;

        for (size_t j = i + 1; j < points.size(); ++j) {
            if (euclideanDistance(points[i], points[j]) < threshold) {
                valid[i] = false;
                valid[j] = false;
            }
        }

        if (valid[i]) {
            filteredPoints.push_back(points[i]);
        }
    }

    return filteredPoints;
}


//不单独用线程，直接在订阅雷达数据的回调函数中处理
void processQueueWithScanCallback(std::vector<std::shared_ptr<ReflectorPanelPoint>> &reflectorPanelPoints) {


    //计算雷达点(已经换算到map下了)
    //  std::cout<<"start 111111111111111transformAndCalculate id: "<<id<<" 1111111111111多线程订阅tf数据11111111111 isMatchok_： "<<isMatchok_<<std::endl;

    if(!is_trilateration_){
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        return;

    }


    //开始时间
    auto start = std::chrono::high_resolution_clock::now();
    {//代码块

        //==========================================================================================
        //todo 通过里程计计算位置 记录日志使用
        tf::Transform mapTbase_link_transformlog =    tf_last_odom_to_map_transform_*base_link_to_odom_transform_;
        geometry_msgs::Pose mapTbase_linklog;
        // 设置位置
        mapTbase_linklog.position.x = mapTbase_link_transformlog.getOrigin().getX();
        mapTbase_linklog.position.y = mapTbase_link_transformlog.getOrigin().getY();
        mapTbase_linklog.position.z = mapTbase_link_transformlog.getOrigin().getZ();

        // 设置方向（四元数）
        // 假设这是一个指向Z轴正方向的单位四元数，表示没有旋转
        mapTbase_linklog.orientation.x = mapTbase_link_transformlog.getRotation().getX();
        mapTbase_linklog.orientation.y = mapTbase_link_transformlog.getRotation().getY();
        mapTbase_linklog.orientation.z = mapTbase_link_transformlog.getRotation().getZ();
        mapTbase_linklog.orientation.w = mapTbase_link_transformlog.getRotation().getW();
        //  tf::Quaternion q11(mapTbase_linklog.orientation.x, mapTbase_linklog.orientation.y, mapTbase_linklog.orientation.z, mapTbase_linklog.orientation.w); //  x, y, z，w, 分量
        //  double reflectorPanelPoints_laserTscan_yaw1   = tf::getYaw(q11);
        double mapTbase_link_yaw_log=tf::getYaw(mapTbase_linklog.orientation); // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
        //===========================================================================================

        if(reflectorPanelPoints.size()==0){//数据为空

            //是否配合第三方定位算法一起使用，比如 amcl等
            if(!isUsingThirdPartyLocalization_){
                double odom_to_map_x= tf_last_odom_to_map_transform_.getOrigin().getX();
                double odom_to_map_y= tf_last_odom_to_map_transform_.getOrigin().getY();
                if(fabs(odom_to_map_x)+fabs(odom_to_map_y)<0.001)
                {
                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    return;
                }
                // todo 3. isSendTf_
                if(isSendTf_|| test_num_!=3)
                    tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));

                // 匹配完成时间
                auto end = std::chrono::high_resolution_clock::now();
                // 计算并转换为毫秒
                auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                // 输出执行时间
                std::cout <<"算法定位： processQueue： 基于之前已经匹配上了反光板的 算法定位， "<< "   执行时间：" << duration.count() << std::endl;
                //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                if(time_log_fig_==0||time_log_fig_==10) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                    std::string resultType = "trilateration_execution";
                    std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [队列 matchokReflectorPanelPoint->reflectorPanelPoints.size()==0]";
                    std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot.[队列 matchokReflectorPanelPoint->reflectorPanelPoints.size()==0]";
                    std::string className = __FILE__; // 更改为实际的文件名
                    int classLine = __LINE__; // 更改为实际的行号
                    string matchReflectorPaneIds="EMPTY";
                    ///trilateration_time_log
                    publishTrilaterationLog(reflectorPanelPoints.size(), duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size() ,matchReflectorPaneIds,mapTbase_linklog,mapTbase_link_yaw_log);

                }


            }//if(!isUsingThirdPartyLocalization_)
            else{
                tf_last_odom_to_map_transform_ =odom_to_map_transform_;

            }

            std::cout <<"TransformException  发布 tf " << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            return;
        }

        ///reflectorPanelPoints.size()
        //===========================================================================================
        //todo 匹配上的id 记录日志使用
        string matchReflectorPaneIds_log="EMPTY";
        for(int i=0;i<reflectorPanelPoints.size();i++){
            if(i==0)
                matchReflectorPaneIds_log=reflectorPanelPoints[i]->id;
            else
                matchReflectorPaneIds_log=matchReflectorPaneIds_log+","+reflectorPanelPoints[i]->id;
        }
        //===========================================================================================


        //todo 2. 之前已经有配准过的数据支持使用一个反光板计算小车定位坐标，使用坐标变化来做
        //matchokReflectorPanelPointEnd_Unordered_map_ ： 储存的是匹配上的点位  map下的反光板全局坐标 std::tuple<x, y> 为key
        //   if(reflectorPanelPoints.size()>0&& reflectorPanelPoints.size()<=2&&firstTrilateration_with_yawFig_){//准确度比 经典三边定位最小二乘法 低，如果数据大于2个以上可以时时使用经典三边定位最小二乘法。
        // todo 测试
        if((reflectorPanelPoints.size()>0&& reflectorPanelPoints.size()<=2&&firstTrilateration_with_yawFig_)||test_num_==13){

            //是否配合第三方定位算法一起使用，比如 amcl等
            if(!isUsingThirdPartyLocalization_){
                double odom_to_map_x= tf_last_odom_to_map_transform_.getOrigin().getX();
                double odom_to_map_y= tf_last_odom_to_map_transform_.getOrigin().getY();
                if(fabs(odom_to_map_x)+fabs(odom_to_map_y)<0.001)
                {
                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    return;
                }

                // todo 6. isSendTf_
                if(isSendTf_|| test_num_!=6)
                    tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));


                // 匹配完成时间
                auto end = std::chrono::high_resolution_clock::now();
                // 计算并转换为毫秒
                auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                // 输出执行时间
                std::cout <<"算法定位： processQueue： 基于之前已经匹配上了反光板的 算法定位， "<< "   执行时间：" << duration.count() << std::endl;
                //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                if(time_log_fig_==0||time_log_fig_==10) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                    std::string resultType = "trilateration_execution";
                    std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [matchokReflectorPanelPointEnd_Unordered_map_.size()==0,calculateBaseLinkToMapTransform2 函数反回 false ]";
                    std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot.[matchokReflectorPanelPointEnd_Unordered_map_.size()==0,calculateBaseLinkToMapTransform2 -> fasle ]";
                    std::string className = __FILE__; // 更改为实际的文件名
                    int classLine = __LINE__; // 更改为实际的行号
                    string matchReflectorPaneIds=matchReflectorPaneIds_log;
                    ///trilateration_time_log
                    publishTrilaterationLog(reflectorPanelPoints.size(), duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_linklog,mapTbase_link_yaw_log);
                }

            }//if(!isUsingThirdPartyLocalization_)
            else{
                tf_last_odom_to_map_transform_ =odom_to_map_transform_;

            }
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
            return;


            //  }else if(reflectorPanelPoints.size()>2){
        }else if(reflectorPanelPoints.size()>findPolygonIndices_threshold_){

//            std::cout <<"---------------------------经典三边定位最小二乘法-------------------------------"  <<std::endl;
//            std::cout <<" 6583  bbbbbbbbbbbbbbbbbbbb matchokReflectorPanelPointEnd_Unordered_map_.size()， "<< matchokReflectorPanelPointEnd_Unordered_map_.size() << std::endl;

//                     if(reflectorPanelPoints.size()<2)
//                         continue;
            //todo 3. 经典三边定位最小二乘法
            Pose   mapTbase_link_pose ;//经典三边定位最小二乘法 得到的移动机器人在map中的位置
            try{

                /**请订阅我的博客，博客中有详细介绍：
                反光柱定位算法中的移动机器人姿态计算原理-经典三边定位最小二乘法计算定位-直接法计算定位
                原文链接：
                https://blog.csdn.net/qq_15204179/article/details/142561692
                */
                // (法一，用向量计算base_link姿态),保证 reflectorPanelPoints.size()>2,否则内存报错 具体看trilateration_with_yaw_new
                mapTbase_link_pose   =   trilateration_with_yaw_new(reflectorPanelPoints);//todo 此时的数据是旧的数据，应为里程计在时时变化，还需要更新里程计数据，6692

                // mapTbase_link_pose   =   trilateration_with_yaw(reflectorPanelPoints);
            }catch (std::exception &e) {
                cout<<"6607 error 经典三边定位最小二乘法: "<<e.what()<<endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                return;
            }

            // (法二，用移动机器人的2点确定一条直线计算base_link姿态)
            // Pose   mapTbase_link_pose   =   trilateration_with_yaw(reflectorPanelPoints);

            std::cout <<" 2  bbbbbbbbbbbbbbbbbbbb matchokReflectorPanelPointEnd_Unordered_map_.size()， "<< matchokReflectorPanelPointEnd_Unordered_map_.size() << std::endl;


            if(fabs(mapTbase_link_pose.position.x)+fabs(mapTbase_link_pose.position.y)<0.00000001){
                std::cout <<" 333 matchokReflectorPanelPointEnd_Unordered_map_.size()， "<< matchokReflectorPanelPointEnd_Unordered_map_.size() << std::endl;



                // todo 9. isSendTf_
                if(isSendTf_|| test_num_!=9) // 时时计算得到的移动机器人坐标 :mapTcurrentCalculateBaseLinkPose_frame_id_  tf_last_odom_to_map_transform_  mapTodom_transform
                    tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));

                // 匹配完成时间
                auto end = std::chrono::high_resolution_clock::now();
                // 计算并转换为毫秒
                auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                // 输出执行时间
                std::cout <<"使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [trilateration_with_yaw_new 数据 mapTbase_link_pose 返回 0. "<< "   执行时间：" << duration.count() << std::endl;
                //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                if(time_log_fig_==0||time_log_fig_==10) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                    std::string resultType = "trilateration_execution";
                    std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [trilateration_with_yaw_new 数据 mapTbase_link_pose 返回 0 ]";
                    std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot.[trilateration_with_yaw_new return mapTbase_link_pose = 0]";
                    std::string className = __FILE__; // 更改为实际的文件名
                    int classLine = __LINE__; // 更改为实际的行号
                    string matchReflectorPaneIds=matchReflectorPaneIds_log;
                    ///trilateration_time_log
                    publishTrilaterationLog(reflectorPanelPoints.size(), duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_linklog,mapTbase_link_yaw_log);
                }

                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                return;
            }




            //通过pose
            Point point  =  mapTbase_link_pose.position;//mapTbase_link

            // todo mapTbase_link 经典三边定位最小二乘法 计算得到的 mapTbase_link
            tf::Transform mapTbase_link_trilateration;
            mapTbase_link_trilateration.setOrigin(tf::Vector3(point.x, point.y, 0.0));
            tf::Quaternion q;
            q.setRPY(0, 0, point.yaw);
            //mapTbase_link
            mapTbase_link_trilateration.setRotation(q);


//                //todo true 表示上一帧数据与下一帧数据存在明明显问题
//                if (isTransformOutOfRange(mapTbase_link_trilateration, up_mapTbase_link_trilateration_, mapTbase_link_pos_threshold_, mapTbase_link_rot_threshold_)) {
//                    firstTrilateration_with_yawFig_=false;
//                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
//                    // continue;
//                }
//
//                //todo 存储上一侦计算得到的定位数据
//                up_mapTbase_link_trilateration_=mapTbase_link_trilateration;
//
            //===========================================================================================
            std::cout <<"====================================== 发布三边定位最小二乘法坐标关系 =================================================="  <<std::endl;

            //记录是否已经定位成功了一次，第一次定位成功就设置true
            if(firstTrilateration_with_yawFig_==false)
                firstTrilateration_with_yawFig_=true;


            //================================================================================================

            //   mapTbase_link_trilateration*
            //    mapTbase_link_trilateration =odom_to_map_transform_ * laser_odomTbaselint_transform;

            //   /**
            // 获取旧的 odomTbaselink的数据。  最新的移动机器人位置  因为里程计在时时变化，所以要更新里程计数据
            tf::StampedTransform laser_to_odom_transform  =  reflectorPanelPoints[0]->last_laser_to_odom_transform_;// laser_to_base_link_transform_

            tf::Transform     laser_odomTbaselint_transform =  laser_to_odom_transform * laser_to_base_link_transform_.inverse();

            tf::Transform     laser_mapTodom_transform =  mapTbase_link_trilateration*laser_odomTbaselint_transform.inverse();

            //todo 最新的移动机器人位置  因为里程计在时时变化，所以要更新里程计数据，重新赋值回 mapTbase_link_trilateration
            mapTbase_link_trilateration = laser_mapTodom_transform * base_link_to_odom_transform_;
            //  */

            //todo 发布 mapTbase_link 用经典三边定位最小二乘法计算的定位 【去对比当前的定位是否存在问题】
            tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_link_trilateration, ros::Time::now(), map_frame_id_, mapTcurrentCalculate_sanbian_BaseLinkPose_frame_id_));


            //是否配合第三方定位算法一起使用，比如 amcl等
            if(!isUsingThirdPartyLocalization_){

                // todo 10. isSendTf_
                if(isSendTf_|| test_num_!=10){//发布坐标关系

                    //todo 发布 mapTbase_link 用经典三边定位最小二乘法计算的定位 【去对比当前的定位是否存在问题】
                    // tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_link_trilateration, ros::Time::now(), map_frame_id_, base_link_frame_id_));

                    //=====================================================
                    // todo 计算  odom 在 map 坐标系下的变换 mapTodom
                    // tf::Transform map_to_odom_transform = base_link_to_map_transform * odom_to_base_link_transform.inverse();
                    // mapTodom  = mapTbase_link * base_linkTodom  , mapTbase_link =  mapTodom * odomTbase_link

                    //todo 发布 map 到 odom 的变换 mapTodom_transform
                    // tf_broadcaster_new_->sendTransform(tf::StampedTransform(laser_mapTodom_transform, ros::Time::now(), map_frame_id_, odom_frame_id_));


                    //   tf::Transform mapTodom_transform = mapTbase_link_trilateration * base_link_to_odom_transform_.inverse();//laser_mapTodom_transform
                    isRunProcessQueueFig_ =false;// 11658 控制 processQueue tf 发布
                    std::this_thread::sleep_for(std::chrono::milliseconds(2));
                    tf::Transform mapTodom_transform = laser_mapTodom_transform;
                    //todo 最后一次定位数据 tf_last_odom_to_map_transform_=mapTodom_transform
                    tf_last_odom_to_map_transform_=mapTodom_transform;//laser_mapTodom_transform
                    //                                                                 mapTodom_transform
                    tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTodom_transform, ros::Time::now(), map_frame_id_, odom_frame_id_));
                    std::this_thread::sleep_for(std::chrono::milliseconds(2));
                    isRunProcessQueueFig_ =true;
                    //========================================================================
                    std::cout <<"100 经典三边定位最小二乘法  发布base_link_to_odom tf 4984  " <<   base_link_to_odom_transform_.getOrigin().getX()<< std::endl;
                    std::cout <<"100 经典三边定位最小二乘法  发布base_link_to_odom tf 4984  " <<   base_link_to_odom_transform_.getOrigin().getY() <<std::endl;

                    std::cout <<"100 经典三边定位最小二乘法  发布 mapTodom_transform tf 4984  " <<   mapTodom_transform.getOrigin().getX()<< std::endl;
                    std::cout <<"100 经典三边定位最小二乘法  发布 mapTodom_transform tf 4984  " <<   mapTodom_transform.getOrigin().getY() <<std::endl;
                    std::cout <<"====================================== =================================================="  <<std::endl;
                    std::cout <<"100 经典三边定位最小二乘法  发布 mapTbase_link_trilateration x 4984  " <<   mapTbase_link_trilateration.getOrigin().getX()<< std::endl;
                    std::cout <<"100 经典三边定位最小二乘法  发布 mapTbase_link_trilateration y 4984  " <<   mapTbase_link_trilateration.getOrigin().getY() <<std::endl;
                    std::cout <<"100 经典三边定位最小二乘法  发布 base_link_to_map_transform getRotation.z() tf 4984  " <<   mapTbase_link_trilateration.getRotation().z() <<std::endl;
                    std::cout <<"100 经典三边定位最小二乘法  发布 base_link_to_map_transform getRotation().w() tf 4984  " <<   mapTbase_link_trilateration.getRotation().w() <<std::endl;
                    std::cout <<"100 经典三边定位最小二乘法  发布 point.yaw tf 4984  " <<   point.yaw <<std::endl;
                    std::cout <<"====================================== =================================================="  <<std::endl;


                }//if(isSendTf_)
            }//    if(!isUsingThirdPartyLocalization_)
            else{//重定位

                ispublishInitialPosefig_ = true;//是否操作的重定位
                //todo 发布 mapTbase_link 用经典三边定位最小二乘法计算的定位 【去对比当前的定位是否存在问题】
                tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_link_trilateration, ros::Time::now(), map_frame_id_, mapTcurrentCalculate_sanbian_BaseLinkPose_frame_id_));

                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                publishInitialPose(mapTbase_link_trilateration, map_frame_id_,initial_pose_pub_);

                //    tf_last_odom_to_map_transform_ =odom_to_map_transform_;



            }//重定位


            // 匹配完成时间
            auto end = std::chrono::high_resolution_clock::now();
            // 计算并转换为毫秒
            auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
            // 输出执行时间
            std::cout <<"算法定位：processQueue： 经典三边定位最小二乘法， "<< " 执行时间：" << duration.count() << std::endl;
            //发布性能评估数据 //   8.经典三边定位最小二乘法，推算位姿
            if(time_log_fig_==0||time_log_fig_==30) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                std::string resultType = "trilateration_execution";
                std::string resultChineseMessage = "经典三边定位最小二乘法计算移动机器人的位置坐标 [ trilateration_with_yaw_new ]";
                std::string resultENMessage = "The classic trilateration least squares method is used to calculate the position coordinates of mobile robots. [ trilateration_with_yaw_new ]";
                std::string className = __FILE__; // 更改为实际的文件名
                int classLine = __LINE__; // 更改为实际的行号
                string matchReflectorPaneIds=matchReflectorPaneIds_log;
                geometry_msgs::Pose mapTbase_link;
                // 设置位置
                mapTbase_link.position.x = mapTbase_link_pose.position.x;
                mapTbase_link.position.y = mapTbase_link_pose.position.y;
                mapTbase_link.position.z = mapTbase_link_pose.position.z;

                tf::Quaternion mapTbase_link_q;
                mapTbase_link_q.setRPY(0, 0, mapTbase_link_pose.position.yaw);
                // 设置方向（四元数）
                // 假设这是一个指向Z轴正方向的单位四元数，表示没有旋转
                mapTbase_link.orientation.x = mapTbase_link_q.getX();
                mapTbase_link.orientation.y = mapTbase_link_q.getY();
                mapTbase_link.orientation.z = mapTbase_link_q.getZ();
                mapTbase_link.orientation.w = mapTbase_link_q.getW();
//                        tf::Quaternion q1(reflectorPanelPoints[i]->laserTscan_rx, reflectorPanelPoints[i]->laserTscan_ry, reflectorPanelPoints[i]->laserTscan_rz, reflectorPanelPoints[i]->laserTscan_rw); //  x, y, z，w, 分量
//                        double reflectorPanelPoints_laserTscan_yaw   = tf::getYaw(q1);
                double mapTbase_link_yaw=mapTbase_link_pose.position.yaw; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                ///trilateration_time_log
                publishTrilaterationLog(reflectorPanelPoints.size(), duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);

            }


        }//if(reflectorPanelPoints.size()>0&& reflectorPanelPoints.size()<=2){}  else if(reflectorPanelPoints.size()>findPolygonIndices_threshold_)

        else{
            if(!isUsingThirdPartyLocalization_){
                double odom_to_map_x= tf_last_odom_to_map_transform_.getOrigin().getX();
                double odom_to_map_y= tf_last_odom_to_map_transform_.getOrigin().getY();
                if(fabs(odom_to_map_x)+fabs(odom_to_map_y)<0.0001)
                {
                    std::this_thread::sleep_for(std::chrono::milliseconds(50));
                    return;
                }

                // todo 1. isSendTf_
                if(isSendTf_|| test_num_!=1){
                    tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));

                    // 匹配完成时间
                    auto end = std::chrono::high_resolution_clock::now();
                    // 计算并转换为毫秒
                    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                    // 输出执行时间
                    std::cout <<"使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标。[mutex]， "<< "   执行时间：" << duration.count() << std::endl;

                    //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                    if(time_log_fig_==0||time_log_fig_==31) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                        std::string resultType = "trilateration_execution";
                        std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标。[mutex]";
                        std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot. [mutex]";
                        std::string className = __FILE__; // 更改为实际的文件名
                        int classLine = __LINE__; // 更改为实际的行号
                        string matchReflectorPaneIds="EMPTY";
                        geometry_msgs::Pose mapTbase_link;
                        double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                        ///trilateration_time_log
                        publishTrilaterationLog(0, duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
                    }
                }//if(isSendTf_)

            }
            else{
                tf_last_odom_to_map_transform_ =odom_to_map_transform_;

            }
        }
        // 休眠一段时间
        std::this_thread::sleep_for(std::chrono::milliseconds(2));
        // lock.unlock();

    }//end 代码块

    std::this_thread::sleep_for(std::chrono::milliseconds(10));

}



//计算方差
double calculateVarianceYaw(const tf::Transform& t1, const tf::Transform& t2) {
    // 计算平移差异
//    tf::Vector3 translation_diff = t1.getOrigin() - t2.getOrigin();
//    double translation_variance = translation_diff.length2();
    double  translation_variance =0.0;
    // 计算旋转差异
    tf::Quaternion rotation_diff = t1.getRotation().inverse() * t2.getRotation();
    double angle = 2 * acos(rotation_diff.w());
    double rotation_variance = angle * angle;

    // 总方差
    return translation_variance + rotation_variance;
}

//计算方差
double calculateVariancePose(const tf::Transform& t1, const tf::Transform& t2) {
    // 计算平移差异
    tf::Vector3 translation_diff = t1.getOrigin() - t2.getOrigin();
    double translation_variance = translation_diff.length2();

//    // 计算旋转差异
//    tf::Quaternion rotation_diff = t1.getRotation().inverse() * t2.getRotation();
//    double angle = 2 * acos(rotation_diff.w());
//    double rotation_variance = angle * angle;
    double rotation_variance =0.0;
    // 总方差
    return translation_variance + rotation_variance;
}

//11505 12686 matchokReflectorPanelPointEnd_Unordered_map_.push_back
//把匹配上的数据加入 matchokReflectorPanelPointEnd_Unordered_map_ 并 赋值后显示出来
void updateMatchedReflectors(
        const std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>, std::shared_ptr<ReflectorPanelPoint>>>& indices,
        const std::vector<Point>& transformed_circle, // Replace YourDataType with the actual type of transformed_circle
        std::string &matchReflectorPaneIds

) {
    for (size_t i = 0; i < indices.size(); ++i) {
        auto &panelPoint = std::get<0>(indices[i]);

        // Update properties
        panelPoint->isAssignment_ = -1;
        panelPoint->last_seen_ = ros::Time::now();
        panelPoint->isMatchok_ = 1;
        panelPoint->currentIsMatchok_ = 1;
        panelPoint->laserTscan_x = transformed_circle[i].laserTscan_x;
        panelPoint->laserTscan_y = transformed_circle[i].laserTscan_y;
        panelPoint->laserTscan_z = transformed_circle[i].laserTscan_z;
        panelPoint->laserTscan_yaw = transformed_circle[i].laserTscan_yaw;
        panelPoint->laserTscan_rx = transformed_circle[i].laserTscan_rx;
        panelPoint->laserTscan_ry = transformed_circle[i].laserTscan_ry;
        panelPoint->laserTscan_rz = transformed_circle[i].laserTscan_rz;
        panelPoint->laserTscan_rw = transformed_circle[i].laserTscan_rw;
        panelPoint->scan_index = transformed_circle[i].scan_index;
        panelPoint->range = transformed_circle[i].range;
        panelPoint->intensity = transformed_circle[i].intensity;

        // Record current odom data
        // panelPoint->last_laser_to_odom_transform_ = now_odomTlaser_transform_; // Assuming this is defined somewhere
        panelPoint->last_seen_ = ros::Time::now();

        panelPoint->isAssignment_ = 1;
        //panelPoint->transformAndCalculate();//显示数据
        // Add matched reflector to unordered_map
        iswirtMatchokReflectoUnordered_map_ = true;
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
        matchokReflectorPanelPointEnd_Unordered_map_[std::make_tuple(panelPoint->x, panelPoint->y)] = panelPoint;
        iswirtMatchokReflectoUnordered_map_ = false;

        // Logging
        if (i == 0) {
            // Initialize matchReflectorPaneIds or use it as needed
            matchReflectorPaneIds = matchReflectorPaneIds;
        } else {
            // Update matchReflectorPaneIds or use it as needed
            matchReflectorPaneIds = matchReflectorPaneIds + "," + matchReflectorPaneIds;

        }
    }
}


//==============================================================

//207 计算距离
inline double ComputeDistance(pair<double, double> &pt1, pair<double, double> &pt2) {
    //相邻的2个全局坐标计算的距离，也就是线段长度，全局坐标系下的坐标对（x, y),
    return sqrt(pow(pt1.first - pt2.first, 2) + pow(pt1.second - pt2.second, 2));
}

// 207 计算半径，局部坐标系下的坐标在laser或baselink的距离，（具体看 pt 数据是基于laser下的还是baselink下的 ）
inline double ComputeRadius(pair<double, double> &pt) {
    return sqrt(pow(pt.first, 2) + pow(pt.second, 2));
}



bool CheckResultValid(
        vector<double> &radius_list,
        vector<pair<pair<double, double>, pair<double, double> > > &match_result,
        pair<double, double> &result, int id1, int id2, double d12) {
    int result_sz = match_result.size();//result_sz: 计算match_result的大小。
    //r1和r2: 分别获取id1和id2对应的半径。
    double r1 = radius_list[id1], r2 = radius_list[id2];
    // D_INFO << "r1: " << r1 << ", r2: " << r2 << ", d12: " << d12;
    //pt1和pt2: 获取id1和id2对应的全局坐标。
    pair<double, double> pt1 = match_result[id1].first, pt2 = match_result[id2].first;
    // D_INFO << "viscircles([" << pt1.first << "," << pt1.second << "]," << r1 << ");";
    // D_INFO << "viscircles([" << pt2.first << "," << pt2.second << "]," << r2 << ");";
    //计算交点:a: 从pt1到交点的投影长度，x0和y0: 从pt1到交点的投影点坐标。h: 交点到投影点的垂直距离。，res_1和res_2: 两个可能的交点坐标。
    double a = (r1 * r1 - r2 * r2 + d12 * d12) / (2 * d12);
    double x0 = pt1.first + a / d12 * (pt2.first - pt1.first);
    double y0 = pt1.second + a / d12 * (pt2.second - pt1.second);

    double h = sqrt(r1 * r1 - a * a);
    pair<double, double> res_1, res_2;
    res_1.first = x0 + h / d12 * (pt2.second - pt1.second);
    res_1.second = y0 - h / d12 * (pt2.first - pt1.first);
    res_2.first = x0 - h / d12 * (pt2.second - pt1.second);
    res_2.second = y0 + h / d12 * (pt2.first - pt1.first);

    // D_INFO << "result 1st: (" << res_1.first << ", " << res_1.second << "), 2nd: ("
    //        << res_2.first << ", " << res_2.second << ")";
    //这个方法返回一个数值，表示在进行匹配时允许的最大距离误差 mapping/full_graph_mapping/full_graph_mapping_test.cc:53:  options.set_distance_match_tolerance(0.05);
    // double distance_match_tolerance = _options.distance_match_tolerance();
    double distance_match_tolerance =edgesthreshold_;
//遍历match_result中的每个匹配结果（除了id1和id2）。
    for (int id = 0; id < result_sz; id++) {
        if (id != id1 && id != id2) {
            // D_INFO << "use id: " << id << " to check result's validation.";
            pair<double, double> pt_check = match_result[id].first;
            double r_check = radius_list[id];
            // D_INFO << "viscircles([" << pt_check.first << "," << pt_check.second << "],"
            //        << r_check << ");";
            //对于每个匹配结果，计算它与res_1和res_2之间的距离差。
            double check_distance_1 = fabs(ComputeDistance(res_1, pt_check) - r_check);//309
            double check_distance_2 = fabs(ComputeDistance(res_2, pt_check) - r_check);
            //check_distance_1和check_distance_2: 分别为res_1和res_2与当前匹配结果之间的距离差。
            //如果距离差小于distance_match_tolerance，则认为匹配结果有效，并将相应的交点坐标赋给result。
            if (check_distance_1 <= check_distance_2) {
                if (check_distance_1 < distance_match_tolerance) {
                    result = res_1;
                    return true;
                }
                std::cout << "the min delt distance: " << check_distance_1 << " is larger than "
                          << distance_match_tolerance << " [REJECT]";
            } else {
                if (check_distance_2 < distance_match_tolerance) {
                    result = res_2;
                    return true;
                }
                std::cout  << "the min delt distance: " << check_distance_2 << " is larger than "
                           << distance_match_tolerance << " [REJECT]";
            }
        }
    }

    std::cout  << "None result has been checked success for: (" << pt1.first << ", " << pt1.second
               << "), (" << pt2.first << ", " << pt2.second << ")";
    return false;
}





/**
 * @brief
 *
 * @param match_result global points - local points
 * @param robot_pose
 * @return true
 * @return false
 这里输入为匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿
 参数:
 match_result: 一个引用到vector容器的参数，其中每个元素都是一个pair，第一个pair包含全局坐标系下的坐标对（x, y），第二个pair包含局部坐标系下的坐标对（x, y）。
 robot_pose: 一个引用到VEnus::Sensor::RobotPose类型的参数，用于设置计算出的机器人位置和朝向。
 返回值: 一个布尔值，表示是否成功计算出了机器人的位置和朝向。

此函数通过分析特征匹配结果来估计机器人的当前位置和朝向。它首先计算每个匹配结果的半径，然后评估所有匹配结果之间的距离，并根据特定条件选择最佳匹配结果来确定机器人的位置和朝向。如果找到了合适的匹配结果，函数会更新robot_pose并返回true；否则，返回false。
这段代码适用于基于特征匹配的机器人定位场景，在实际应用中可以根据具体需求进行调整。

 */
bool ComputeCurrentPose(
        //match_result 第一个pair包含全局坐标系下的坐标对（x, y），第二个pair包含局部坐标系下的坐标对（x, y）。
        vector<pair<pair<double, double>, pair<double, double> > > &match_result,
        //VEnus::Sensor::RobotPose &robot_pose
        Pose &robot_pose
) {
    int result_sz = match_result.size();//result_sz: 计算match_result的大小。
    vector<double> radius_list;//radius_list: 一个用于存储每个匹配结果的半径的vector。
    //遍历match_result，对于每个匹配结果，调用ComputeRadius函数计算半径，并将结果存储在radius_list中。
    for (int i = 0; i < result_sz; i++) {

        /**
         207
        inline double CartoMapping::ComputeRadius(pair<double, double> &pt) {
        return sqrt(pow(pt.first, 2) + pow(pt.second, 2));
      }
        */
        //match_result[i].second 第二个pair包含局部坐标系下的坐标对（x, y）
        radius_list.push_back(ComputeRadius(match_result[i].second));//207
    }

    for (int i = 0; i < result_sz - 1; i++) {
        for (int j = i + 1; j < result_sz; j++) {//内层循环遍历match_result中的每一对匹配结果，计算它们之间的距离dij。
            //相邻的2个全局坐标的线段长度
            double dij = ComputeDistance(match_result[i].first, match_result[j].first);//309 计算距离
            //相邻的2个局部坐标系下的坐标在laser或baselink的距离（具体看 match_result[i].second 数据是基于laser下的还是baselink下的 ）
            double score = (radius_list[i] + radius_list[j]) / dij;//计算一个分数score，它是两个半径之和除以距离dij
            //如果score大于1.2且小于等于3.0，调用CheckResultValid函数来验证匹配结果是否有效。
            if (score > score_min_threshold_ && score <= score_max_threshold_) {//如果score大于1.2且小于等于3.0，调用CheckResultValid函数来验证匹配结果是否有效。
                pair<double, double> res;
                //254 这个函数的主要目的是验证两个匹配结果是否能够共同确定一个合理的机器人位置
                if (CheckResultValid(radius_list, match_result, res, i, j, dij)) {//如果CheckResultValid返回true，则计算机器人的位置和朝向，并更新robot_pose。
                    // initial x, y
                    // global position
                    pair<double, double> global_tar_pt = match_result[i].first;//计算全局目标点global_tar_pt相对于计算出的位置res的角度angle_global。
                    pair<double, double> local_pt = match_result[i].second;//计算局部点local_pt的角度angle_local
                    ////计算初始角度initial_value_angle，即angle_global减去angle_local，并对结果进行归一化处理，确保其在[-2π, 2π)范围内。
                    double angle_global =
                            atan2(global_tar_pt.second - res.second, global_tar_pt.first - res.first);
                    double angle_local = atan2(local_pt.second, local_pt.first);

                    double initial_value_angle = angle_global - angle_local;
                    if (initial_value_angle > 2 * M_PI) {
                        initial_value_angle -= 2 * M_PI;
                    } else if (initial_value_angle < -2 * M_PI) {
                        initial_value_angle += 2 * M_PI;
                    }
                    //更新robot_pose的位置和朝向，并返回true。如果没有找到有效的匹配结果，函数返回false。
                    robot_pose.position.x=res.first;
                    robot_pose.position.y=res.second;
                    robot_pose.position.yaw=initial_value_angle;
                    return true;
                }
            }
        }
    }

    return false;
}


bool ComputeCurrentPose(
        //match_result 第一个pair包含全局坐标系下的坐标对（x, y），第二个pair包含局部坐标系下的坐标对（x, y）。
        vector<pair<pair<double, double>, pair<double, double> > > &match_result,
        //VEnus::Sensor::RobotPose &robot_pose
        Pose &robot_pose,
        double &result_score
) {
    int result_sz = match_result.size();//result_sz: 计算match_result的大小。
    vector<double> radius_list;//radius_list: 一个用于存储每个匹配结果的半径的vector。
    //遍历match_result，对于每个匹配结果，调用ComputeRadius函数计算半径，并将结果存储在radius_list中。
    for (int i = 0; i < result_sz; i++) {

        /**
         207
        inline double CartoMapping::ComputeRadius(pair<double, double> &pt) {
        return sqrt(pow(pt.first, 2) + pow(pt.second, 2));
      }
        */
        //match_result[i].second 第二个pair包含局部坐标系下的坐标对（x, y）
        radius_list.push_back(ComputeRadius(match_result[i].second));//207
    }

    for (int i = 0; i < result_sz - 1; i++) {
        for (int j = i + 1; j < result_sz; j++) {//内层循环遍历match_result中的每一对匹配结果，计算它们之间的距离dij。
            //相邻的2个全局坐标的线段长度
            double dij = ComputeDistance(match_result[i].first, match_result[j].first);//309 计算距离
            //相邻的2个局部坐标系下的坐标在laser或baselink的距离（具体看 match_result[i].second 数据是基于laser下的还是baselink下的 ）
            double score = (radius_list[i] + radius_list[j]) / dij;//计算一个分数score，它是两个半径之和除以距离dij
            result_score=score;
            //如果score大于1.2且小于等于3.0，调用CheckResultValid函数来验证匹配结果是否有效。
            if (score > score_min_threshold_ && score <= score_max_threshold_) {//如果score大于1.2且小于等于3.0，调用CheckResultValid函数来验证匹配结果是否有效。
                pair<double, double> res;
                //254 这个函数的主要目的是验证两个匹配结果是否能够共同确定一个合理的机器人位置
                if (CheckResultValid(radius_list, match_result, res, i, j, dij)) {//如果CheckResultValid返回true，则计算机器人的位置和朝向，并更新robot_pose。
                    // initial x, y
                    // global position
                    pair<double, double> global_tar_pt = match_result[i].first;//计算全局目标点global_tar_pt相对于计算出的位置res的角度angle_global。
                    pair<double, double> local_pt = match_result[i].second;//计算局部点local_pt的角度angle_local
                    ////计算初始角度initial_value_angle，即angle_global减去angle_local，并对结果进行归一化处理，确保其在[-2π, 2π)范围内。
                    double angle_global =
                            atan2(global_tar_pt.second - res.second, global_tar_pt.first - res.first);
                    double angle_local = atan2(local_pt.second, local_pt.first);

                    double initial_value_angle = angle_global - angle_local;
                    if (initial_value_angle > 2 * M_PI) {
                        initial_value_angle -= 2 * M_PI;
                    } else if (initial_value_angle < -2 * M_PI) {
                        initial_value_angle += 2 * M_PI;
                    }
                    //更新robot_pose的位置和朝向，并返回true。如果没有找到有效的匹配结果，函数返回false。
                    robot_pose.position.x=res.first;
                    robot_pose.position.y=res.second;
                    robot_pose.position.yaw=initial_value_angle;
                    return true;
                }
            }
        }
    }

    return false;
}




//=============================================================



//#include <Eigen/Dense>
//#include <tf/transform_datatypes.h>

tf::Transform  fuseTransforms(const tf::Transform& mapTbase_link_trilateration_new,
                              const tf::StampedTransform& mapTbase_link_transform)
{
    // 提取通过反光板计算得到的位姿
    Vector3d state_trilateration(
            mapTbase_link_trilateration_new.getOrigin().x(),
            mapTbase_link_trilateration_new.getOrigin().y(),
            tf::getYaw(mapTbase_link_trilateration_new.getRotation()));

    // 提取通过tf订阅得到的位姿
    Vector3d state_tf(
            mapTbase_link_transform.getOrigin().x(),
            mapTbase_link_transform.getOrigin().y(),
            tf::getYaw(mapTbase_link_transform.getRotation()));

    // 假设测量噪声协方差矩阵
    Matrix3d R_trilateration = Eigen::Matrix3d::Identity() * 0.5; // 假设反光板计算的噪声较大
    Matrix3d R_tf = Eigen::Matrix3d::Identity() * 0.1;            // 假设tf订阅的噪声噪声较小

    // 先验估计（可以是上次融合的结果，也可以初始化为其中一个测量值）
    Vector3d state_prior = state_tf; // 这里初始化为tf订阅的值
    Matrix3d P_prior = Eigen::Matrix3d::Identity() * 0.1; // 初始协方差

    // 计算卡尔曼增益
    Matrix3d S = P_prior + R_tf;
    Matrix3d K = P_prior * S.inverse();

    // 融合两个位姿
    Vector3d state_post = state_prior + K * (state_trilateration - state_prior);

    // 更新协方差
    Matrix3d P_post = (Eigen::Matrix3d::Identity() - K) * P_prior;

    // 将融合后的位姿转换回tf::Transform
    tf::Transform fused_transform;
    fused_transform.setOrigin(tf::Vector3(state_post(0), state_post(1), 0.0));
    tf::Quaternion fused_q;
    fused_q.setRPY(0, 0, state_post(2));
    fused_transform.setRotation(fused_q);
    return fused_transform;
    // 你可以在这里使用或发布fused_transform
    // publishFusedTransform(fused_transform); // 假设你有一个函数发布融合后的结果
}



//todo 当前 baselink对应的  result_Mapvalue_， result_Mapvalue： 直接发得到的baselink对应的值

/**
 这段代码可以被命名为 evaluateTrilaterationTimeout，其功能主要在于评估三边定位过程是否超时，并根据不同的条件记录和输出相应的日志信息。以下是命名的一些考虑因素：
功能描述：该函数主要关注于三边定位算法的超时评估。
逻辑清晰：通过检查不同条件来决定是否需要重新开始定位过程。
日志记录：根据配置参数 time_log_fig_ 决定是否记录详细的超时信息。
因此，evaluateTrilaterationTimeout 这个名称能够很好地概括这段代码的主要职责。
 */
void  evaluateTrilaterationTimeout(){

    /// else if ((isGetResultMapvalueCallbackfig_ && result_Mapvalue_!=free_cell_)||(isGetCalculateScanMapRatioCallbackfig_&& !is_ratio_)){
    //经典三边定位最小二乘法 每次计算得到的 mapTbase_link 的j结束时间
    std::chrono::time_point<std::chrono::high_resolution_clock> trilateration_mapTbase_link_pose_endTime= std::chrono::high_resolution_clock::now();
    // 计算并转换为毫秒
    auto trilateration_ms = std::chrono::duration_cast<std::chrono::milliseconds>(trilateration_mapTbase_link_pose_endTime - trilateration_mapTbase_link_pose_startTime_);
    // auto trilateration_s = std::chrono::duration_cast<std::chrono::seconds>(trilateration_ms);//转成秒
    double trilateration_s = static_cast<double>(trilateration_ms.count()) / 1000.0;//转成秒    //targetTrilaterationTimeOut_ = 20
    double targetTrilaterationTimeOut = targetTrilaterationTimeOut_/5;

    //发布评估数据
    std::string resultType = "MatchokReflectorPanelPoint_scan_CB";
    std::string resultChineseMessage ="基于反光柱特征的匹配算法：广度优先算法+优先使用匹配好的反光板数据 [匹配数据]";
    std::string resultENMessage = "Matching algorithm: breadth first algorithm for matching+prioritizing the use of well matched reflector data [MatchokReflectorPanelPoint]";
    std::string className = __FILE__; // 更改为实际的文件名
    std::string  matchReflectorPaneIds ="EMPTY";
    int classLine = __LINE__; // 更改为实际的行号
    if(time_log_fig_==17) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

        std::string message = " 17 未计算出有效的定位数据导致超时，targetTrilaterationTimeOut_ ：" + std::to_string(targetTrilaterationTimeOut_)+ " s， 当前累计时间：" + std::to_string(trilateration_s) + " s， 超时阀值 ：" + std::to_string(targetTrilaterationTimeOut)+" s, | " ;



        if(isTrilateration_with_yawFig_)
            message=message+"isTrilateration_with_yawFig_： true ,";
        else
            message=message+"isTrilateration_with_yawFig_： false ,";

        if(firstTrilateration_with_yawFig_)
            message=message+"firstTrilateration_with_yawFig_： true ,";
        else
            message=message+"firstTrilateration_with_yawFig_： false ,";


        if(isGetResultMapvalueCallbackfig_)
            message=message+"isGetResultMapvalueCallbackfig_： true ,";
        else
            message=message+"isGetResultMapvalueCallbackfig_： false ,";


        if(isGetCalculateScanMapRatioCallbackfig_)
            message=message+"isGetCalculateScanMapRatioCallbackfig_： true ,";
        else
            message=message+"isGetCalculateScanMapRatioCallbackfig_： false ,";

        double ratio1 =0;
        if(scan_points_map_size_!=0){
            ratio1 = static_cast<double>(matching_points_) / scan_points_map_size_;
        }
        message=message+"  | 当前 baselink 在地图中的代价值result_Mapvalue_ ："+std::to_string(result_Mapvalue_)+",在地图轮廓上的雷达点数 matching_points_" + std::to_string(matching_points_) + " ， 所有雷达点数 scan_points_map_size_ ：" + std::to_string(scan_points_map_size_);


        bool is_ratio1 = (ratio1 >= matchingcostma_threshold_);
        message=message+ " | 匹配雷达数据比例超时，匹配雷达数据比例 ratio：" + std::to_string(ratio1) + " ， 匹配雷达数据比例超时阀值 matchingcostma_threshold_：" + std::to_string(matchingcostma_threshold_);
        if(is_ratio1)
            message=message+"，匹配雷达数据比例超时，is_ratio： true ,";
        else
            message=message+"，匹配雷达数据比例超时，is_ratio： false ,";


        // 输出执行时间
        std::cout <<message << std::endl;
        resultChineseMessage =message+ "[计算定位数据数据超时]";
        resultENMessage = "Failure to calculate valid positioning data resulted in timeout, current cumulative time" + std::to_string(trilateration_s) + " s， Timeout threshold ：" + std::to_string(targetTrilaterationTimeOut_)+" s";
        className = __FILE__; // 更改为实际的文件名
        classLine = __LINE__; // 更改为实际的行号
        geometry_msgs::Pose mapTbase_link;
        double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
        // /trilateration_time_log
        publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
    }


    if(trilateration_s>targetTrilaterationTimeOut_){ //超时重新计算  targetTrilaterationTimeOut_
        trilateration_mapTbase_link_pose_startTime_=std::chrono::high_resolution_clock::now();
        //isTrilateration_with_yawFig_=true;//超时重新计算
        firstTrilateration_with_yawFig_=false;
        if(time_log_fig_==14) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

            std::string message = "14 未计算出有效的定位数据导致超时，当前累计时间：" + std::to_string(trilateration_s) + " s， 超时阀值 ：" + std::to_string(targetTrilaterationTimeOut_)+" s";
            // 输出执行时间
            std::cout <<message << std::endl;
            resultChineseMessage =message+ "[计算定位数据数据超时]";
            resultENMessage = "Failure to calculate valid positioning data resulted in timeout, current cumulative time" + std::to_string(trilateration_s) + " s， Timeout threshold ：" + std::to_string(targetTrilaterationTimeOut_)+" s";
            className = __FILE__; // 更改为实际的文件名
            classLine = __LINE__; // 更改为实际的行号
            geometry_msgs::Pose mapTbase_link;
            double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
            // /trilateration_time_log
            publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
        }

    }

    if(trilateration_s>targetTrilaterationTimeOut && isGetResultMapvalueCallbackfig_ && result_Mapvalue_!=free_cell_){//判断当前机器人的定位数据是否不满足条件，不满足则重新计算 （机器人不在代价地图为0的自由区）
        // trilateration_mapTbase_link_pose_startTime_=std::chrono::high_resolution_clock::now();
        isTrilateration_with_yawFig_=true;//超时重新计算
        firstTrilateration_with_yawFig_=false;


        if(time_log_fig_==15) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

            std::string message = " 15 未计算出有效的定位数据导致超时，当前 baselink 在地图中的代价值result_Mapvalue_ ：" + std::to_string(result_Mapvalue_) ;
            // 输出执行时间
            std::cout <<message << std::endl;
            resultChineseMessage =message+ "[计算定位数据数据超时]";
            resultENMessage = "Failure to calculate valid positioning data resulted in timeout, current cumulative time" + std::to_string(trilateration_s) + " s， Timeout threshold ：" + std::to_string(targetTrilaterationTimeOut_)+" s";
            className = __FILE__; // 更改为实际的文件名
            classLine = __LINE__; // 更改为实际的行号
            geometry_msgs::Pose mapTbase_link;
            double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
            // /trilateration_time_log
            publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
        }

    }


    if(trilateration_s>targetTrilaterationTimeOut && isGetCalculateScanMapRatioCallbackfig_ ){//判断当前机器人的雷达点数在地图轮廓上的比例是否满足条件，不满足则重新计算


        double ratio =0;
        //if(scan_points_map_size_!=0|| total_points_size_!=0){


        if(scan_points_map_size_!=0){

            //scan_points_map_size_= true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，
            // 需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。
            if(is_use_scan_points_map_size_){
                 ratio = static_cast<double>(matching_points_) / scan_points_map_size_;
            }else{
                ratio = static_cast<double>(matching_points_) / total_points_size_;//计算雷达点数据量的有效数据量与地图轮廓的匹配率
            }


        }

        bool is_ratio = (ratio >= matchingcostma_threshold_);
        if(!is_ratio){
            // trilateration_mapTbase_link_pose_startTime_=std::chrono::high_resolution_clock::now();
            isTrilateration_with_yawFig_=true;//超时重新计算
            firstTrilateration_with_yawFig_=false;

            if(time_log_fig_==16) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                std::string message = "15 未计算出有效的定位数据导致超时 , 雷达数据比例 ratio：" + std::to_string(ratio) + " ， 超时阀值 matchingcostma_threshold_：" + std::to_string(matchingcostma_threshold_);
                // 输出执行时间
                std::cout <<message << std::endl;
                resultChineseMessage =message+ "[计算定位数据数据超时]";
                resultENMessage = "Failure to calculate valid positioning data resulted in timeout, current cumulative matching_points_" + std::to_string(matching_points_) + " ， scan_points_map_size_ ：" + std::to_string(scan_points_map_size_);
                className = __FILE__; // 更改为实际的文件名
                classLine = __LINE__; // 更改为实际的行号
                geometry_msgs::Pose mapTbase_link;
                double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                // /trilateration_time_log
                publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
            }

        }

    }

}// evaluateTrilaterationTimeout()


//------------------------------------------------------------------------
bool scan_CB_new1(const sensor_msgs::LaserScan::ConstPtr& scanMsg) {
    // 尝试非阻塞上锁
    std::unique_lock<std::mutex> lock(scancallback_mutex_, std::try_to_lock);
    if (!lock.owns_lock()||!matchokReflectorPanelPoint_.empty()) {

        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        return false;
    }


    if(unorderedEdgeMapNew_.size()==0){

        std::cerr << "请部署反光板 unorderedEdgeMapNew_.size(): " <<unorderedEdgeMapNew_.size()<< std::endl;
        return false;
    }


    try {
        //todo ================================================================
        //todo start 统计时间
        auto start1 = std::chrono::high_resolution_clock::now();
        //todo ================================================================

        if(getScanParam_){//获取雷达参数
            getScanParam_= false;
            scan_size_ = scanMsg->ranges.size();
            intensities_size_ =  scanMsg->intensities.size();
            laser_frame_id_=scanMsg->header.frame_id;
        }
        string matchReflectorPaneIds="EMPTY";

        std::vector< Point > unprocessed_circle;//雷达坐标系下的反光板坐标

        // scan_CB_new1 获取雷达坐标系到odom坐标系的变换   odomTlaser
        tf::StampedTransform odomTlaser_transform_1;
        // scan_CB_new1 获取雷达坐标系到地图坐标系的变换   mapTlaser
        tf::StampedTransform transform_map_laser_1;
        tf::StampedTransform odomTbase_link_transform_1;
        tf::StampedTransform mapTbase_link_transform_1;
        try {
            //todo laserTodom
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            tf_listener_new_->waitForTransform(odom_frame_id_, laser_frame_id_,ros::Time(0), ros::Duration(1.0));
            tf_listener_new_->lookupTransform(odom_frame_id_, laser_frame_id_, ros::Time(0), odomTlaser_transform_1);

            tf_listener_new_->lookupTransform(map_frame_id_, scanMsg->header.frame_id, ros::Time(0), transform_map_laser_1);

            tf_listener_new_->lookupTransform(odom_frame_id_,  base_link_frame_id_,ros::Time(0), odomTbase_link_transform_1);

            tf_listener_new_->lookupTransform(map_frame_id_,  base_link_frame_id_,ros::Time(0), mapTbase_link_transform_1);

            now_odomTlaser_transform_=odomTlaser_transform_1;
            base_link_to_odom_transform_=odomTbase_link_transform_1;
            base_link_to_map_transform_=mapTbase_link_transform_1;
        } catch (tf::TransformException& ex) {
            ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

        }
        //todo scan_CB_new1 1. 部署是获取的反光板的数据 unprocessed_circle
        // unprocessed_circle = findCircle(*scanMsg);
        //查找高强度点组作为反光柱并确用三角法确定圆心
        unprocessed_circle =  findCircle (*scanMsg,odomTlaser_transform_1,transform_map_laser_1,odomTbase_link_transform_1);


        std::vector<Point> circle = filterPoints(unprocessed_circle, filter_tow_ont_distance_threshold_);

        if(circle.size()<=2){

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            return false;
        }



        //发布评估数据
        std::string resultType = "MatchokReflectorPanelPoint_scan_CB";
        std::string resultChineseMessage ="基于反光柱特征的匹配算法：广度优先算法+优先使用匹配好的反光板数据 [匹配数据]";
        std::string resultENMessage = "Matching algorithm: breadth first algorithm for matching+prioritizing the use of well matched reflector data [MatchokReflectorPanelPoint]";
        std::string className = __FILE__; // 更改为实际的文件名
        int classLine = __LINE__; // 更改为实际的行号

        //todo end 统计时间
        // 匹配完成时间 scan_CB_new1
        //1：FindCircle: 通过雷达强度数据，使用三角函数拟定圆心坐标，并存入反光板数据缓存中
        if(time_log_fig_==-2||time_log_fig_==0||time_log_fig_==1){//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。
            //发布性能评估数据

            auto end1 = std::chrono::high_resolution_clock::now();
            // 计算并转换为毫秒
            auto duration1 = std::chrono::duration_cast<std::chrono::milliseconds>(end1 - start1);

            resultType = "MatchokReflectorPanelPoint_scan_CB";
            resultChineseMessage = "FindCircle: 通过雷达强度数据，过滤反光柱数据，并存入反光板数据缓存中：std::vector< Point > circle 【获取的反光柱数据】";
            resultENMessage = "FindCircle: Using radar intensity data, use trigonometric functions to determine the center coordinates of the circle and store them in the reflector data cache: std:: vector<Point>circle【Obtained reflective column data】";
            className = __FILE__; // 更改为实际的文件名
            classLine = __LINE__; // 更改为实际的行号
            geometry_msgs::Pose mapTbase_link;
            double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
            ///trilateration_time_log
            // publishTrilaterationLog(0, duration2.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,reflectorPanelPointAllUnordered_maps_.size());
            publishTrilaterationLog(0, duration1.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);

        }


        if(!is_trilateration_ ){
            previous_base_link_to_odom_transform_=base_link_to_odom_transform_;
            previous_base_link_to_map_transform_ =base_link_to_map_transform_;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            //std::cerr << "is_trilateration_=false 没有开启反光板定位或是没扫描到反光板 circle.size(): " <<circle.size()<< std::endl;
            return false;
        }



        //todo 3. 匹配反光板
        //scan_CB_new1 匹配开始时间
        auto start2 = std::chrono::high_resolution_clock::now();

        std::cout <<"反光柱匹配算法"<< std::endl;

        auto start4 = std::chrono::high_resolution_clock::now();


        if(circle.size()<=2){
            //     if(transformed_circle.size()<=2){
            std::cout <<"匹配反光板,雷达扫描到的反光板数量小于2个，不进行匹配,transformed_circle.size(): "<< circle.size() <<std::endl;

            return false;
        }


        std::vector<double> targetEdges;
        std::vector<double> targetRadius;
        //  bool targetEdgesFig = calculateTargetEdges(transformed_circle,targetEdges);
        bool targetEdgesFig = calculateTargetEdges(circle,targetEdges);
        // bool targetEdgesFig = calculateTargetEdges(circle,targetEdges,targetRadius);
        if(!targetEdgesFig)
            return false;

        std::vector<double> targetAngles;
        bool targetAnglesFig = calculateTargetAngles(circle,targetAngles);
        if(isStartAreAnglesEqualNew_&&!targetAnglesFig)
            return false;




        std::map<int,std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> >>  map_targetIndices_rusults;//储存多个多边形数据，所有数据都通过三边算法计算，从中间选择一个最贴近目前baselink的数据；
        bool indicesfig = findPolygonIndicesNewWithQueueNew1(unorderedEdgeMapNew_, targetEdges,targetAngles, edgesthreshold_,map_targetIndices_rusults);

        auto end4 = std::chrono::high_resolution_clock::now();
        // 计算并转换为毫秒
        auto duration4 = std::chrono::duration_cast<std::chrono::milliseconds>(end4 - start4);
        //scan_CB_new1 11:测试
        if(time_log_fig_==-2|| time_log_fig_==0||time_log_fig_==11) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

            std::string message = "反光柱匹配算法：只用特征匹配算法  需要匹配的边长数量：" + std::to_string(targetEdges.size()) + " 条， 从查找圆心到匹配完成需要的总执行时间 ：" + std::to_string(duration4.count())+" milliseconds";

            if(isReadMatchokReflectorPanel_){
                message = "  存在上一次的匹配数据时优先使用匹配好的反光板数据 + 匹配走特征匹配算法混合匹配模式 =》 需要匹配的边长数量：" + std::to_string(targetEdges.size()) + "  条， 从查找圆心到匹配完成需要的总执行时间 ： " + std::to_string(duration4.count())+" milliseconds";
            }

            std::cout <<message<< std::endl;
            // 输出执行时间
            std::cout <<message << std::endl;
            resultChineseMessage =message+ "[匹配数据]";
            resultENMessage = "Reflective Column Matching Algorithm [MatchokReflectorPanelPoint]";
            className = __FILE__; // 更改为实际的文件名
            classLine = __LINE__; // 更改为实际的行号
            geometry_msgs::Pose mapTbase_link;
            double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
            // /trilateration_time_log
            publishTrilaterationLog(0, duration4.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
        }





        if(!indicesfig||map_targetIndices_rusults.size()==0){
            previous_base_link_to_odom_transform_=base_link_to_odom_transform_;
            previous_base_link_to_map_transform_ =base_link_to_map_transform_;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));

            evaluateTrilaterationTimeout();
            //--------------------------------------------------------------------------------------
            if(indicesfig){
                std::cout << RED<<"indicesfig: true , =================== return false;  3边定位计算失败 findPolygonIndicesNewWithQueueNew1 ==========if(!indicesfig||map_targetIndices_rusults.size()==0)================map_targetIndices_rusults.size(): "<<map_targetIndices_rusults.size()<< RESET<<std::endl;

            }else{
                std::cout << RED<< " indicesfig: false ,=================== return false;  3边定位计算失败 findPolygonIndicesNewWithQueueNew1 ==========if(!indicesfig||map_targetIndices_rusults.size()==0)================map_targetIndices_rusults.size(): "<<map_targetIndices_rusults.size()<< RESET<<std::endl;
            }

            return false;
        }



        std::cout <<GREEN<< "======================================================================================================================================"<<std::endl;
        int num=1;
        int  minKey= -1;
        double min_varianceYaw = std::numeric_limits<double>::max();//匹配数据方差
        double min_variancePose = std::numeric_limits<double>::max();//匹配数据方差
        tf::Transform   mapTbase_transform_min ;//找到方差最小的一个数据 移动机器人定位数据
        tf::StampedTransform last_laser_to_odom_transform_min;

        int result_Mapvalue = -3;
        bool handleGetResultMapvalue_fig=false;
        double max_ratio = 0.0;//雷达与地图轮廓的匹配率
        bool handleGetCalculateScanMapRatio_fig=false;

        int matching_points=0;//匹配到的数据量
        int scan_points_map_size=0;//雷达点数据量
        int total_points_size=0;//雷达点有效数据量
        bool is_ratio=false;//是否满足匹配要求
        double ratio =10;

        // todo mapTbase_link 经典三边定位最小二乘法 计算得到的 mapTbase_link
        //tf::Transform mapTbase_link_trilateration_new;
        //tf::Transform mapTbase_link_old_trilateration;
        std::cout << "共找到：" <<map_targetIndices_rusults.size() << "个多边形特征数据" <<std::endl;
        // 遍历所有多边形
        for (const auto& entry : map_targetIndices_rusults) {
            matchReflectorPaneIds="EMPTY";
            std::vector<ReflectorPanelPointStruct> structIndices_first ;//储存的所有匹配好的反光板点（临时，需要从众多匹配数据中筛选一个出来，如果用std::shared_ptr<ReflectorPanelPoint>，可能会有脏数据，所有用临时数据）
            vector<pair<pair<double, double>, pair<double, double> > > localization_hit;//储存的所有匹配好的反光板点（临时，需要从众多匹配数据中筛选一个出来，如果用std::shared_ptr<ReflectorPanelPoint>，可能会有脏数据，所有用临时数据）
            int key = entry.first; // 获取键
            const std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> >& indices = entry.second;
            std::cout << "第 " <<num<<" 条,key:"<<key<< ",path数据：" <<std::endl;
            num++;
            if(indices.size()<=findPolygonIndices_threshold_ ||  indices.size()!=circle.size()){
                std::cout << "匹配失败，跳过 findPolygonIndices_threshold_" <<findPolygonIndices_threshold_<<std::endl;
                std::cout << "匹配失败，跳过 indices.size()" <<indices.size()<<std::endl;

                std::cout << "匹配失败，跳过 transformed_circle.size()" <<circle.size()<<std::endl;

                continue;
            }

            tf::StampedTransform odomTlaser_now_transform;//laserTodom
            try {
                //todo scan_CB_new1 laserTodom
                tf_listener_new_->waitForTransform(odom_frame_id_, laser_frame_id_,ros::Time(0), ros::Duration(1.0));
                tf_listener_new_->lookupTransform(odom_frame_id_, laser_frame_id_, ros::Time(0), odomTlaser_now_transform);
                now_odomTlaser_transform_=odomTlaser_now_transform;

            } catch (tf::TransformException& ex) {
                ROS_ERROR("scan_CB_new1 ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

            }

            for (int i = 0; i < indices.size(); ++i) {
                ReflectorPanelPointStruct reflectorPanelPointStruct;
                reflectorPanelPointStruct.id=std::get<0>(indices[i])->id;
                //部署的反光柱数据 mapT反光柱
                reflectorPanelPointStruct.x=std::get<0>(indices[i])->x;
                reflectorPanelPointStruct.y=std::get<0>(indices[i])->y;
                reflectorPanelPointStruct.z=std::get<0>(indices[i])->z;
                reflectorPanelPointStruct.rx=std::get<0>(indices[i])->rx;
                reflectorPanelPointStruct.ry=std::get<0>(indices[i])->ry;
                reflectorPanelPointStruct.rz=std::get<0>(indices[i])->rz;
                reflectorPanelPointStruct.rw=std::get<0>(indices[i])->rw;
                reflectorPanelPointStruct.yaw=std::get<0>(indices[i])->yaw;
                //明天在 ReflectorPanelPointStruct 这里通过tf 时时转换 laserTscan_x 数据，雷达探测的原始数据在reflectorPanelPointStruct.circle中
                reflectorPanelPointStruct.circle=circle[i];////雷达探测到的当前反光柱数据
                reflectorPanelPointStruct.laserTscan_x=circle[i].laserTscan_x;
                reflectorPanelPointStruct.laserTscan_y=circle[i].laserTscan_y;
                reflectorPanelPointStruct.laserTscan_z=circle[i].laserTscan_z;
                reflectorPanelPointStruct.laserTscan_yaw=circle[i].laserTscan_yaw;
                reflectorPanelPointStruct.laserTscan_rx=circle[i].laserTscan_rx;
                reflectorPanelPointStruct.laserTscan_ry=circle[i].laserTscan_ry;
                reflectorPanelPointStruct.laserTscan_rz=circle[i].laserTscan_rz;
                reflectorPanelPointStruct.laserTscan_rw=circle[i].laserTscan_rw;
                reflectorPanelPointStruct.scan_index=circle[i].scan_index;
                reflectorPanelPointStruct.range=circle[i].range;
                reflectorPanelPointStruct.intensity=circle[i].intensity;
                reflectorPanelPointStruct.last_seen_=ros::Time::now();//当前匹配上的时间
                reflectorPanelPointStruct.last_laser_to_odom_transform_=circle[i].odomTlaser_transform_1;//  odomTlaser 记录匹配上时的历程数据
                reflectorPanelPointStruct.last_mapTbase_link_transform_=circle[i].transform_map_laser_1;//记录匹配上时的历程数据
                reflectorPanelPointStruct.last_odomTbase_link_transform_=circle[i].odomTbase_link_transform_1;
                //scan_CB_new1 class ReflectorPanelPoint
                // Update properties
                std::get<0>(indices[i])->isAssignment_ = -1;
                std::get<0>(indices[i])->circle=circle[i];//雷达探测到的当前反光柱数据
                std::get<0>(indices[i])->last_seen_ = ros::Time::now();
                std::get<0>(indices[i])->last_laser_to_odom_transform_=circle[i].odomTlaser_transform_1;
                std::get<0>(indices[i])->last_mapTbase_link_transform_=circle[i].transform_map_laser_1;
                std::get<0>(indices[i])->last_odomTbase_link_transform_=circle[i].odomTbase_link_transform_1;
                std::get<0>(indices[i])->last_seen_ = ros::Time::now();
                std::get<0>(indices[i])->isAssignment_ = 1;


                //todo 1. 上一次 通过雷达探测到的实时 odom坐标系下的反光柱坐标
                tf::Transform previous_odomTscanpoint_transform;
                previous_odomTscanpoint_transform.setOrigin(tf::Vector3(circle[i].odomTscan_x, circle[i].odomTscan_y, circle[i].odomTscan_z ));
                //tf::Quaternion q;
                tf::Quaternion previous_odomTscanpoint_q(circle[i].odomTscan_rx, circle[i].odomTscan_ry, circle[i].odomTscan_rz, circle[i].odomTscan_rw); //  x, y, z，w, 分量
                // q.setRPY(0, 0, reflectorPanelPoint->laserTscan_yaw);
                previous_odomTscanpoint_transform.setRotation(previous_odomTscanpoint_q);//base_link_to_odom_transform;//odomTbase_link

                tf::Transform  laserTscanPoint_now = odomTlaser_now_transform.inverse() * previous_odomTscanpoint_transform;

                tf::Transform  base_linkTfanguangban_transform  = laser_to_base_link_transform_ * laserTscanPoint_now;
                //base_linkTfanguangban
                double base_linkTfanguangban_to_map_x=  base_linkTfanguangban_transform.getOrigin().getX();
                double base_linkTfanguangban_to_map_y=  base_linkTfanguangban_transform.getOrigin().getY();
                double base_linkTfanguangban_to_map_z=  base_linkTfanguangban_transform.getOrigin().getZ();

                //scan_CB_new1 当前时时扫描到的反光柱在baselink下的坐标 【通过这个值可以求移动机器人的姿态或者，全局反光柱的当前姿态】
                reflectorPanelPointStruct.current_baselinkTscan_x=base_linkTfanguangban_to_map_x;
                reflectorPanelPointStruct.current_baselinkTscan_y=base_linkTfanguangban_to_map_y;
                reflectorPanelPointStruct.current_baselinkTscan_z=base_linkTfanguangban_to_map_z;
                reflectorPanelPointStruct.current_baselinkTscan_rx=0.0;
                reflectorPanelPointStruct.current_baselinkTscan_ry=0.0;
                reflectorPanelPointStruct.current_baselinkTscan_rz=0.0;
                reflectorPanelPointStruct.current_baselinkTscan_rw=1.0;
                reflectorPanelPointStruct.base_linkTfanguangban_distances =  std::sqrt(std::pow(base_linkTfanguangban_to_map_x , 2) + std::pow(base_linkTfanguangban_to_map_y, 2) );

                if (std::isnan(reflectorPanelPointStruct.base_linkTfanguangban_distances) || std::isinf(reflectorPanelPointStruct.base_linkTfanguangban_distances)||reflectorPanelPointStruct.base_linkTfanguangban_distances == 0)
                    return false;



                structIndices_first.push_back(reflectorPanelPointStruct);//给最小二乘法三边定位
                //------------------------------------------------------------------------------------------------------------------------------
                //scan_CB_new1 创建pair对，存储全局坐标和局部坐标 [参考： VEnus]
                pair<double, double> global_coords(reflectorPanelPointStruct.x, reflectorPanelPointStruct.y);
                pair<double, double> local_coords(base_linkTfanguangban_to_map_x, base_linkTfanguangban_to_map_y);

                //todo 将pair对存储到localization_hit 中,ComputeCurrentPose(localization_hit, robot_pose) 匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿
                localization_hit.push_back(make_pair(global_coords, local_coords));

                //日志使用
                if(i==0)
                    matchReflectorPaneIds=reflectorPanelPointStruct.id;
                else
                    matchReflectorPaneIds=matchReflectorPaneIds+","+matchReflectorPaneIds;

            }
            std::cout << "-------------------------------------------------------------"<<RESET<<std::endl;
            // 打印识别完成的反光柱
            printIndices( indices);


            //scan_CB_new1 12 是测试
            if(time_log_fig_==-2|| time_log_fig_==0||time_log_fig_==13) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                std::string message = "反光柱匹配算法：只用广度优先特征匹配算法  需要匹配的边长数量：" + std::to_string(targetEdges.size()) + " 条， 从查找圆心到匹配完成需要的总执行时间 ：" + std::to_string(duration4.count())+" milliseconds";

                if(isReadMatchokReflectorPanel_){
                    message = " 5599 存在上一次的匹配数据时优先使用匹配好的反光板数据 + 匹配走广度优先特征匹配算法混合匹配模式 =》 需要匹配的边长数量：" + std::to_string(targetEdges.size()) + "  条， 从查找圆心到匹配完成需要的总执行时间 ： " + std::to_string(duration4.count())+" milliseconds";
                }

                std::cout <<message<< std::endl;
                // 输出执行时间
                std::cout <<message << std::endl;
                resultChineseMessage =message+ "[匹配数据]";
                resultENMessage = "Reflective Column Matching Algorithm [MatchokReflectorPanelPoint]";
                className = __FILE__; // 更改为实际的文件名
                classLine = __LINE__; // 更改为实际的行号
                geometry_msgs::Pose mapTbase_link;
                double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                // /trilateration_time_log
                publishTrilaterationLog(0, duration4.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);

            }


            tf::StampedTransform mapTbase_link_transform;//mapTbase_link
            try {
                //todo laserTodom
                tf_listener_new_->waitForTransform(map_frame_id_, base_link_frame_id_,ros::Time(0), ros::Duration(1.0));
                tf_listener_new_->lookupTransform(map_frame_id_, base_link_frame_id_, ros::Time(0), mapTbase_link_transform);
                base_link_to_map_transform_=mapTbase_link_transform;

            } catch (tf::TransformException& ex) {
                ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

            }

            Pose robot_pose;
            //从tf获取
            Pose last_pose;
            last_pose.position.x=mapTbase_link_transform.getOrigin().x();
            last_pose.position.y=mapTbase_link_transform.getOrigin().y();
            last_pose.position.yaw=tf::getYaw(mapTbase_link_transform.getRotation());
            double result_score=0.0;
            bool computeCurrentPoseFig=false;
            //todo 这里输入为 ComputeCurrentPose(localization_hit, robot_pose) 匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿
            if (!ComputeCurrentPose(localization_hit, robot_pose,result_score)) {
                // localization failed...
                robot_pose = last_pose;

                auto endComputeCurrentPose = std::chrono::high_resolution_clock::now();
                // 计算并转换为毫秒
                auto durationendComputeCurrentPose = std::chrono::duration_cast<std::chrono::milliseconds>(endComputeCurrentPose - start1);

                if(time_log_fig_==23) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                    std::string message = "计算大致的机器人位姿：预估失败，使用tf中的mapTbase_link ，ComputeCurrentPose函数中匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿，执行时间 ：" + std::to_string(durationendComputeCurrentPose.count())+" milliseconds";
                    std::cout <<message<< std::endl;
                    // 输出执行时间
                    std::cout <<message << std::endl;
                    resultChineseMessage =message+ "[计算大致的机器人位姿]";
                    resultENMessage = "Compute the approximate robot pose: The current matched reflectors and the set of global reflectors, returning an approximate robot pose, execution time: " + std::to_string(durationendComputeCurrentPose.count()) + " milliseconds";

                    className = __FILE__; // 更改为实际的文件名
                    classLine = __LINE__; // 更改为实际的行号

                    geometry_msgs::Pose mapTbase_link;
                    mapTbase_link.position.x = mapTbase_link_transform.getOrigin().x();
                    mapTbase_link.position.y = mapTbase_link_transform.getOrigin().y();
                    mapTbase_link.position.z = mapTbase_link_transform.getOrigin().z();  // 如果你需要设定z轴
                    tf::Quaternion quat;
                    quat.setRPY(0, 0, last_pose.position.yaw);  // 根据yaw设置四元数
                    mapTbase_link.orientation.x = mapTbase_link_transform.getRotation().x();
                    mapTbase_link.orientation.y = mapTbase_link_transform.getRotation().y();
                    mapTbase_link.orientation.z = mapTbase_link_transform.getRotation().z();
                    mapTbase_link.orientation.w = mapTbase_link_transform.getRotation().w();

                    // /trilateration_time_log
                    publishTrilaterationLog(0, durationendComputeCurrentPose.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,localization_hit.size(),matchReflectorPaneIds,mapTbase_link,robot_pose.position.yaw,computeCurrentPoseFig,result_score);

                }



                std::cout << "robot pose initial pose get failed, use last pose as initial value.";
                // return false;
                //continue;
            }else{

                if(time_log_fig_==23) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                    auto endComputeCurrentPose = std::chrono::high_resolution_clock::now();
                    // 计算并转换为毫秒
                    auto durationendComputeCurrentPose = std::chrono::duration_cast<std::chrono::milliseconds>(
                            endComputeCurrentPose - start1);

                    std::string message =
                            "计算大致的机器人位姿：预估成功 ，ComputeCurrentPose函数中匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿，执行时间 ：" +
                            std::to_string(durationendComputeCurrentPose.count()) + " milliseconds";
                    std::cout << message << std::endl;
                    // 输出执行时间
                    std::cout << message << std::endl;
                    resultChineseMessage = message + "[计算大致的机器人位姿]";
                    resultENMessage =
                            "Compute the approximate robot pose: The current matched reflectors and the set of global reflectors, returning an approximate robot pose, execution time: " +
                            std::to_string(durationendComputeCurrentPose.count()) + " milliseconds";

                    className = __FILE__; // 更改为实际的文件名
                    classLine = __LINE__; // 更改为实际的行号

                    geometry_msgs::Pose mapTbase_link;
                    mapTbase_link.position.x = last_pose.position.x;
                    mapTbase_link.position.y = last_pose.position.y;
                    mapTbase_link.position.z = 0.0;  // 如果你需要设定z轴
                    tf::Quaternion quat;
                    quat.setRPY(0, 0, last_pose.position.yaw);  // 根据yaw设置四元数
                    mapTbase_link.orientation.x = quat.x();
                    mapTbase_link.orientation.y = quat.y();
                    mapTbase_link.orientation.z = quat.z();
                    mapTbase_link.orientation.w = quat.w();
                    computeCurrentPoseFig=true;
                    // /trilateration_time_log
                    publishTrilaterationLog(0, durationendComputeCurrentPose.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,localization_hit.size(),matchReflectorPaneIds,mapTbase_link,robot_pose.position.yaw,computeCurrentPoseFig,result_score);
                }

            }//if (!ComputeCurrentPose(localization_hit, robot_pose,result_score))

            std::cout << "11871 scan_CB_new1 经典三边定位最小二乘法" <<std::endl;
            //todo 3. 经典三边定位最小二乘法
            Pose   mapTbase_link_pose ;//经典三边定位最小二乘法 得到的移动机器人在map中的位置
            try{



                // (法一，用向量计算base_link姿态),保证 reflectorPanelPoints.size()>2,否则内存报错 具体看trilateration_with_yaw_new
                //     mapTbase_link_pose   =   trilateration_with_yaw_new(indices_first);//todo 此时的数据是旧的数据，应为里程计在时时变化，还需要更新里程计数据，6692


                // mapTbase_link_pose   =   optimizePoseWithTrilateration(structIndices_first,robot_pose);//直接法
                //   if(isTrilateration_with_yawFig_){

                //与 trilateration_with_yaw_new 对比，增加了通过里程计数据动态计算反光柱与baseLink的距离
                //     mapTbase_link_pose   =   trilateration_with_yaw_new(structIndices_first);//new 最小二乘三边定位
                //todo 可用
                //  mapTbase_link_pose   =   trilateration_with_yaw_new(structIndices_first);//new 最小二乘三边定位
                //  }else{

                //与trilateration_with_yaw_new对比，增加了通过里程计数据动态计算反光柱与baseLink的距离

                /** todo 请订阅我的博客，博客中有详细介绍：
                  反光柱定位算法中的移动机器人姿态计算原理-经典三边定位最小二乘法计算定位-直接法计算定位
                  原文链接：
                  https://blog.csdn.net/qq_15204179/article/details/142561692
                  */

                //todo 效果好
                mapTbase_link_pose   =   optimizePoseWithTrilateration_new1(structIndices_first,robot_pose);//直接法

//                previous_base_link_to_odom_transform_=base_link_to_odom_transform_;
//                previous_base_link_to_map_transform_ =base_link_to_map_transform_;
            }catch (std::exception &e) {
                previous_base_link_to_odom_transform_=base_link_to_odom_transform_;
                previous_base_link_to_map_transform_ =base_link_to_map_transform_;
                cout<<"6607 error 经典三边定位最小二乘法: "<<e.what()<<endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
                return false;
            }




            //确保数据可用
            if(fabs(mapTbase_link_pose.position.x)+fabs(mapTbase_link_pose.position.y)<0.00001){
                cout<<"8277 error 经典三边定位最小二乘法: "<<endl;
                cout<<"8277 error 经典三边定位最小二乘法: "<<endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
                return false;

            }

            //scan_CB_new1 通过pose
            Point point  =  mapTbase_link_pose.position;//mapTbase_link
            // todo mapTbase_link 经典三边定位最小二乘法 计算得到的 mapTbase_link
            tf::Transform mapTbase_link_trilateration_new;
            mapTbase_link_trilateration_new.setOrigin(tf::Vector3(point.x, point.y, 0.0));
            //mapTbase_link_trilateration_new_.setOrigin(tf::Vector3(point.x, point.y, 0.0));
            tf::Quaternion q;
            q.setRPY(0, 0, point.yaw);
            //mapTbase_link
            mapTbase_link_trilateration_new.setRotation(q);

            mapTbase_link_trilateration_new_=mapTbase_link_trilateration_new;
            // 转换 Transform 到 Pose
            geometry_msgs::Pose mapTbase_link_trilateration_new_pose = transformToPose(mapTbase_link_trilateration_new);

            //scan_CB_new1 计算方差  35.8329
            double varianceYaw = calculateVarianceYaw(mapTbase_link_trilateration_new, mapTbase_link_transform);
            double variancePose = calculateVariancePose(mapTbase_link_trilateration_new, mapTbase_link_transform);
            std::cout <<" key:"<<key<< "，三边定位得到的数据 mapTbase_link：( x： " << point.x << ", y: " << point.y << ", yaw: " << point.yaw << ")" << std::endl;
            double mapTbase_link_yaw=tf::getYaw(mapTbase_link_transform.getRotation()); // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
            std::cout << "当前 mapTbase_link：( x：" << mapTbase_link_transform.getOrigin().getX()<< ", y:  " << mapTbase_link_transform.getOrigin().getY() << " ,yaw: " << mapTbase_link_yaw << ")" << std::endl;

            std::cout <<"方差 varianceYaw:"<<varianceYaw<< std::endl;
            std::cout <<"方差 variancePose:"<<variancePose<< std::endl;
            std::cout <<"======================================================================================"<< std::endl;
            //scan_CB_new1 firstTrilateration_with_yawFig_=false 第一次启动。 firstTrilateration_with_yawFig_=true时需要判断当前数据与上一次定位数据是否合理

            //-------------------------------------------------查询costmap数据--------------------------------------------------------------------------
            //scan_CB_new1 创建一个服务消息对象,判断移动机器人计算出来的坐标数据是否在自由移动区域
            fanguangbantrilateration::matchcostmapwithscan_srvs matchcostmapwithscan_srv;
            matchcostmapwithscan_srv.request.request_type = "handleGetResultMapvalue";  // 设置请求类型
            matchcostmapwithscan_srv.request.request_posint_x= point.x;
            matchcostmapwithscan_srv.request.request_posint_y= point.y;
            matchcostmapwithscan_srv.request.request_posint_z= 0;
            //表示等待5秒。如果服务在5秒内没有准备好，则会抛出错误。
//            if (!match_costmap_with_scan_service_client_.waitForExistence(ros::Duration(0.05))) {  // 5.0 等待5秒
//                ROS_ERROR("Service %s not available after waiting for 5 seconds", "match_costmap_with_scan_service");
//                return false;
//            }
            // 发送服务请求
            if (match_costmap_with_scan_service_client_.call(matchcostmapwithscan_srv))
            {
                result_Mapvalue= matchcostmapwithscan_srv.response.result_Mapvalue;
                //  if( matchcostmapwithscan_srv.response.success){
                handleGetResultMapvalue_fig=true;
                // }

            }
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            //scan_CB_new1 查看雷达数据与cotmap的结合情况
            matchcostmapwithscan_srv.request.request_type = "handleGetCalculateScanMapRatio";  // 设置请求类型
            matchcostmapwithscan_srv.request.request_mapTbase_trilateration_transform=mapTbase_link_trilateration_new_pose;
            matchcostmapwithscan_srv.request.request_radius= matchingcostma_radius_;
            matchcostmapwithscan_srv.request.request_threshold= matchingcostma_threshold_;
            //表示等待5秒。如果服务在5秒内没有准备好，则会抛出错误。
//            if (!match_costmap_with_scan_service_client_.waitForExistence(ros::Duration(0.05))) {  // 5.0 等待5秒
//                ROS_ERROR("Service %s not available after waiting for 5 seconds", "match_costmap_with_scan_service");
//                return false;
//            }

            if (match_costmap_with_scan_service_client_.call(matchcostmapwithscan_srv))
            {
                result_Mapvalue= matchcostmapwithscan_srv.response.result_Mapvalue;
                //   if( matchcostmapwithscan_srv.response.success){
                handleGetCalculateScanMapRatio_fig=true;
                // }

                matching_points=matchcostmapwithscan_srv.response.resultmatching_points_size;//匹配到雷达点数据量
                scan_points_map_size=matchcostmapwithscan_srv.response.scan_points_map_size;//雷达点数据量
                total_points_size=matchcostmapwithscan_srv.response.resulttotal_points_size;//雷达点数据量的有效数据量
                is_ratio=matchcostmapwithscan_srv.response.isCalculateScanMapRatio;//是否满足匹配要求

                //if(scan_points_map_size_!=0|| total_points_size_!=0){
                if(scan_points_map_size!=0){
                    //scan_points_map_size_= true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，
                    // 需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。
                    if(is_use_scan_points_map_size_){
                        ratio = static_cast<double>(matching_points) / scan_points_map_size;//计算所有雷达数据量与地图轮廓的匹配率
                    }else{
                        ratio = static_cast<double>(matching_points) / total_points_size;//计算雷达点数据量的有效数据量与地图轮廓的匹配率

                    }

                }

            }
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
            //---------------------------------------------------------------------------------------------------------------------------

            if(handleGetCalculateScanMapRatio_fig) {
                //todo scan_CB_new1  result_Mapvalue： 直接发得到的baselink对应的值。   result_Mapvalue_ ： 当前 baselink对应的
                //  free_cell_ =  0代表空闲
                if (   ratio > max_ratio &&(result_Mapvalue==free_cell_|| !handleGetResultMapvalue_fig)  ) { //如果只有一个数据就默认最小值

                    max_ratio=ratio;
                    //  if ((varianceYaw < min_varianceYaw  ) ||map_targetIndices_rusults.size()==1) { //如果只有一个数据就默认最小值
                    minKey= key;//距离最短的数据
                    min_varianceYaw = varianceYaw;//找方差最小的数据
                    min_variancePose=variancePose;
                    mapTbase_transform_min = mapTbase_link_trilateration_new_;//找到方差最小的一个数据 移动机器人定位数据

                    if(structIndices_first.size()>0)
                        last_laser_to_odom_transform_min=structIndices_first[0].last_laser_to_odom_transform_;
                    else
                        last_laser_to_odom_transform_min=now_odomTlaser_transform_;//  odomTlaser 记录匹配上时的历程数据

                    std::cout << "-------------对比多边形特征数据----------------------"<<std::endl;
                    std::cout << "雷达点数据量：" <<scan_points_map_size<< "匹配到雷达点数据量： matching_points： " <<min_varianceYaw<<" ,雷达与地图轮廓的匹配率 ratio： " <<ratio<< " ,雷达与地图轮廓的匹配 阀值： matchingcostma_threshold： " <<matchingcostma_threshold_<< " ,雷达与地图轮廓的匹配 搜索半径： matchingcostma_radius_： " <<matchingcostma_radius_<<std::endl;

                }

            }else{
                //scan_CB_new1  free_cell_ =  0代表空闲
                if (varianceYaw < min_varianceYaw && variancePose < min_variancePose &&(result_Mapvalue==free_cell_|| !handleGetResultMapvalue_fig)  ) { //如果只有一个数据就默认最小值
                    //  if ((varianceYaw < min_varianceYaw  ) ||map_targetIndices_rusults.size()==1) { //如果只有一个数据就默认最小值
                    minKey= key;//距离最短的数据
                    min_varianceYaw = varianceYaw;//找方差最小的数据
                    min_variancePose=variancePose;
                    mapTbase_transform_min = mapTbase_link_trilateration_new_;//找到方差最小的一个数据 移动机器人定位数据

                    if(structIndices_first.size()>0)
                        last_laser_to_odom_transform_min=structIndices_first[0].last_laser_to_odom_transform_;
                    else
                        last_laser_to_odom_transform_min=now_odomTlaser_transform_;//  odomTlaser 记录匹配上时的历程数据


                    std::cout << "-------------对比多边形特征数据----------------------"<<RESET<<std::endl;
                    std::cout << "对比多边形特征数据：" <<map_targetIndices_rusults.size() << "个多边形特征数据，方差： min_varianceYaw： " <<min_varianceYaw<<" ,min_variancePose： " <<min_variancePose<< " ,minKey： " <<minKey<<std::endl;

                }




            }// if(handleGetCalculateScanMapRatio_fig)



            //-----------------------------------------------------------------------------------------------------------------------
            //todo scan_CB_new1 发布所有匹配上的移动机器人数据，方便对比出问题来 【正式版本需要注释掉】
            string mapTcurrentCalculate_sanbian_BaseLinkPose_frame_id_new =std::to_string(num)+"_"+mapTcurrentCalculate_sanbian_BaseLinkPose_frame_id_;
            tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_transform_min, ros::Time::now(), map_frame_id_, mapTcurrentCalculate_sanbian_BaseLinkPose_frame_id_new));
            //-----------------------------------------------------------------------------------------------------------------------

        }//todo scan_CB_new1 for (const auto& entry : map_targetIndices_rusults) 遍历所有多边形
        //判断计算出来的  mapTbase_transform_min 中的雷达与地图轮廓的匹配虑配率是否满足要求
        // if((handleGetCalculateScanMapRatio_fig &&  max_ratio<matchingcostma_threshold_)|| (handleGetResultMapvalue_fig && result_Mapvalue!=free_cell_))
        if( max_ratio<matchingcostma_threshold_||   result_Mapvalue!=free_cell_) {
            //     if(handleGetResultMapvalue_fig&&result_Mapvalue!=free_cell_) {

            // evaluateTrilaterationTimeout();


            //scan_CB_new1 发布评估数据
            resultType = "MatchokReflectorPanelPoint_scan_CB";
            resultChineseMessage ="基于反光柱特征的匹配算法：广度优先算法+优先使用匹配好的反光板数据 [匹配数据]";
            resultENMessage = "Matching algorithm: breadth first algorithm for matching+prioritizing the use of well matched reflector data [MatchokReflectorPanelPoint]";
            className = __FILE__; // 更改为实际的文件名
            matchReflectorPaneIds ="EMPTY";
            classLine = __LINE__; // 更改为实际的行号
            if(time_log_fig_==18) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                std::string message = " 18 return 未计算出有效的定位数据导致超时" ;

                if(isTrilateration_with_yawFig_)
                    message=message+"isTrilateration_with_yawFig_： true ,";
                else
                    message=message+"isTrilateration_with_yawFig_： false ,";

                if(firstTrilateration_with_yawFig_)
                    message=message+"firstTrilateration_with_yawFig_： true ,";
                else
                    message=message+"firstTrilateration_with_yawFig_： false ,";


                if(handleGetCalculateScanMapRatio_fig)
                    message=message+"handleGetCalculateScanMapRatio_fig： true ,";
                else
                    message=message+"handleGetCalculateScanMapRatio_fig： false ,";


                if(handleGetResultMapvalue_fig)
                    message=message+"handleGetResultMapvalue_fig： true ,";
                else
                    message=message+"handleGetResultMapvalue_fig： false ,";

                double ratio1 =0;
                if(scan_points_map_size!=0){
                    ratio1 = static_cast<double>(matching_points) / scan_points_map_size;
                }
                message=message+"  | 当前 baselink 在地图中的代价值 result_Mapvalue ："+std::to_string(result_Mapvalue)+",在地图轮廓上的雷达点数 matching_points" + std::to_string(matching_points) + " ， 所有雷达点数 scan_points_map_size ：" + std::to_string(scan_points_map_size);


                bool is_ratio1 = (ratio1 >= matchingcostma_threshold_);
                message=message+ " | 匹配雷达数据比例超时，匹配雷达数据比例 max_ratio：" + std::to_string(max_ratio) +" , ratio1：" + std::to_string(ratio1) + " ， 匹配雷达数据比例超时阀值 matchingcostma_threshold_：" + std::to_string(matchingcostma_threshold_);
                if(is_ratio1)
                    message=message+"，匹配雷达数据比例超时，is_ratio： true ,";
                else
                    message=message+"，匹配雷达数据比例超时，is_ratio： false ,";


                // 输出执行时间
                std::cout <<message << std::endl;
                resultChineseMessage =message+ "[计算定位数据数据超时]";
                resultENMessage = "Failure to calculate valid positioning data resulted in timeout, current cumulative timeTimeout threshold";
                className = __FILE__; // 更改为实际的文件名
                classLine = __LINE__; // 更改为实际的行号
                geometry_msgs::Pose mapTbase_link;
                double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                // /trilateration_time_log
                publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
            }


            return false;
        }else {
            // if(!handleGetCalculateScanMapRatio_fig && !handleGetResultMapvalue_fig ){

            //scan_CB_new1 发布评估数据
            resultType = "MatchokReflectorPanelPoint_scan_CB";
            resultChineseMessage ="基于反光柱特征的匹配算法：广度优先算法+优先使用匹配好的反光板数据 [匹配数据]";
            resultENMessage = "Matching algorithm: breadth first algorithm for matching+prioritizing the use of well matched reflector data [MatchokReflectorPanelPoint]";
            className = __FILE__; // 更改为实际的文件名
            matchReflectorPaneIds ="EMPTY";
            classLine = __LINE__; // 更改为实际的行号
            if(time_log_fig_==19) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....



                std::string message = " 19 算出有效的定位数据" ;

                if(isTrilateration_with_yawFig_)
                    message=message+"isTrilateration_with_yawFig_： true ,";
                else
                    message=message+"isTrilateration_with_yawFig_： false ,";

                if(firstTrilateration_with_yawFig_)
                    message=message+"firstTrilateration_with_yawFig_： true ,";
                else
                    message=message+"firstTrilateration_with_yawFig_： false ,";


                if(handleGetCalculateScanMapRatio_fig)
                    message=message+"handleGetCalculateScanMapRatio_fig： true ,";
                else
                    message=message+"handleGetCalculateScanMapRatio_fig： false ,";


                if(handleGetResultMapvalue_fig)
                    message=message+"handleGetResultMapvalue_fig： true ,";
                else
                    message=message+"handleGetResultMapvalue_fig： false ,";

                double ratio1 =0;
                if(scan_points_map_size!=0){
                    ratio1 = static_cast<double>(matching_points) / scan_points_map_size;
                }
                message=message+"  | 当前 baselink 在地图中的代价值 result_Mapvalue ："+std::to_string(result_Mapvalue)+",在地图轮廓上的雷达点数 matching_points" + std::to_string(matching_points) + " ， 所有雷达点数 scan_points_map_size ：" + std::to_string(scan_points_map_size);


                bool is_ratio1 = (ratio1 >= matchingcostma_threshold_);
                message=message+ " | 匹配雷达数据比例超时，匹配雷达数据比例 max_ratio：" + std::to_string(max_ratio) +" , ratio1：" + std::to_string(ratio1) + " ， 匹配雷达数据比例超时阀值 matchingcostma_threshold_：" + std::to_string(matchingcostma_threshold_);
                if(is_ratio1)
                    message=message+"，匹配雷达数据比例超时，is_ratio： true ,";
                else
                    message=message+"，匹配雷达数据比例超时，is_ratio： false ,";


                // 输出执行时间
                std::cout <<message << std::endl;
                resultChineseMessage =message+ "[计算定位数据数据]";
                resultENMessage = "Failure to calculate valid positioning data resulted in timeout, current cumulative timeTimeout threshold";
                className = __FILE__; // 更改为实际的文件名
                classLine = __LINE__; // 更改为实际的行号
                geometry_msgs::Pose mapTbase_link;
                double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                // /trilateration_time_log
                publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
            }


        }


        std::cout << "=========================================================================="<<RESET<<std::endl;
        std::cout << "===================end for (const auto& entry : map_targetIndices_rusults)=================================================="<<RESET<<std::endl;


        //scan_CB_new1  std::vector<std::shared_ptr<ReflectorPanelPoint>> indices_first ;//储存的所有匹配好的反光板点（确定匹配完成的数据）
        const std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> >& indices = map_targetIndices_rusults[minKey];

        //---------------------------------------------------------------


        //--------------------------------------------------------
        tf::StampedTransform base_link_to_odom_transform;//odomTbase_link
        try {

            //todo  odomTbase_link
            tf_listener_new_->waitForTransform(odom_frame_id_, base_link_frame_id_,ros::Time(0), ros::Duration(1.0));
            tf_listener_new_->lookupTransform(odom_frame_id_, base_link_frame_id_, ros::Time(0), base_link_to_odom_transform);
            base_link_to_odom_transform_=base_link_to_odom_transform;

        } catch (tf::TransformException& ex) {
            ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

        }


        //--------------------------------------------------------------------
        previous_base_link_to_odom_transform_=base_link_to_odom_transform;
        previous_base_link_to_map_transform_ =base_link_to_map_transform_;



        //=====================================================================


        // tf::StampedTransform laser_to_odom_transform  =  last_laser_to_odom_transform_min;// laser_to_base_link_transform_
        if(circle.size()==0)
            return false;
        tf::Transform     previous_odomTlaser_transform_1 =  circle[0].odomTbase_link_transform_1;


        tf::StampedTransform laser_to_odom_transform  =  circle[0].odomTbase_link_transform_1;// laser_to_base_link_transform_
        tf::Transform     odomTbaselint_transform =  laser_to_odom_transform * laser_to_base_link_transform_.inverse();

        // tf::Transform     odomTbaselint_transform =  laser_to_odom_transform * laser_to_base_link_transform_.inverse();
        //   tf::Transform     previous_mapTodom_transform =  mapTbase_transform_min*odomTbase_link_transform_1.inverse();
        tf::Transform     mapTodom_transform =  mapTbase_transform_min*odomTbase_link_transform_1.inverse();//   mapTbase_link *  base_linkTodom


        if (
                (min_varianceYaw < variance_yaw_threshold_ &&  min_variancePose < variance_pose_threshold_)
                //||(!firstTrilateration_with_yawFig_&&(min_varianceYaw < variance_adjustment_yaw_threshold_*15&&  min_variancePose < variance_adjustment_pose_threshold_*10))
                // ||!firstTrilateration_with_yawFig_
                //  ||(!firstTrilateration_with_yawFig_&&!isTrilateration_with_yawFig_)
                ||(isTrilateration_with_yawFig_)
                ||(!firstTrilateration_with_yawFig_&&!isTrilateration_with_yawFig_&&(min_varianceYaw < variance_adjustment_yaw_threshold_&&  min_variancePose < variance_adjustment_pose_threshold_))

//                ||!firstTrilateration_with_yawFig_
                )
        {

            //经典三边定位最小二乘法 每次计算得到的 mapTbase_link 的开始时间
            trilateration_mapTbase_link_pose_startTime_ = std::chrono::high_resolution_clock::now();

            //scan_CB_new1 计算方差  35.8329
            double varianceYaw_min = calculateVarianceYaw(mapTbase_transform_min, base_link_to_map_transform_);
            double variancePose_min = calculateVariancePose(mapTbase_transform_min, base_link_to_map_transform_);





            tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_transform_min, ros::Time::now(), map_frame_id_, mapTcurrentCalculate_sanbian_BaseLinkPose_frame_id_));
            //加入队列
            OptimizePoseWithTrilaterationPose optimizePoseWithTrilaterationPose;
            optimizePoseWithTrilaterationPose.stamp=ros::Time::now();
            optimizePoseWithTrilaterationPose.mapTbase_transform_min=mapTbase_transform_min;
            optimizePoseWithTrilaterationPose.last_laser_to_odom_transform=circle[0].odomTlaser_transform_1;
            optimizePoseWithTrilaterationPose.last_mapTlaser_transform=circle[0].transform_map_laser_1;
            optimizePoseWithTrilaterationPose.last_odomTbase_link_transform=circle[0].odomTbase_link_transform_1;

            optimizePoseWithTrilaterationPose.indices=indices;
            optimizePoseWithTrilaterationPose.circle=circle;
            optimizePoseWithTrilaterationPose.matchReflectorPaneIds=matchReflectorPaneIds;

            optimizePoseWithTrilaterationPose_queue_.push_back(optimizePoseWithTrilaterationPose);

            //scan_CB_new1 是否配合第三方定位算法一起使用，比如 amcl等
            if(!isUsingThirdPartyLocalization_){
                std::cout << "//是否配合第三方定位算法一起使用，比如 amcl等.   tf_last_odom_to_map_transform_=laser_mapTodom_transform; " <<std::endl;
                // todo 10. isSendTf_
//                if(isSendTf_|| test_num_!=10) {//发布坐标关系
//
//                    isRunProcessQueueFig_ =false;// 11658 控制 processQueue tf 发布
//                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
//                    tf_last_odom_to_map_transform_=mapTodom_transform;//laser_mapTodom_transform
//                    //scan_CB_new1
//                    tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTodom_transform, ros::Time::now(), map_frame_id_, odom_frame_id_));
//                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
//                    isRunProcessQueueFig_ =true;
//                }

            }
            else{
                ispublishInitialPosefig_ = true;//是否操作的重定位
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
                publishInitialPose(mapTbase_transform_min, map_frame_id_,initial_pose_pub_);

            }




            //scan_CB_new1 程序启动的第一次使用三边定位
            if(isTrilateration_with_yawFig_ && min_varianceYaw < variance_adjustment_yaw_threshold_&&  min_variancePose < variance_adjustment_pose_threshold_)
                isTrilateration_with_yawFig_=false;

            //scan_CB_new1 记录是否已经定位成功了一次，第一次定位成功就设置 true  【firstTrilateration_with_yawFig_=false不绑定当前baselink的定位数据,需要重新绑定】
            if(firstTrilateration_with_yawFig_==false && isTrilateration_with_yawFig_==false )
                //  if(firstTrilateration_with_yawFig_==false )
                firstTrilateration_with_yawFig_=true;//【firstTrilateration_with_yawFig_=true 绑定当前baselink的定位数据，参考base_link_to_map_transform_数据，计算出来的base_link_to_map要在base_link_to_map_transform_数据数据范围内】



            //scan_CB_new1 找到与质心最短距离的多边形
            std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> > targetIndices_min_rusult=  map_targetIndices_rusults[minKey];
            double mapTbase_link_yaw1=tf::getYaw(base_link_to_map_transform_.getRotation()); // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw

            for (int i = 0; i < targetIndices_min_rusult.size(); ++i) {
                const std::shared_ptr<ReflectorPanelPoint>& firstPoint = std::get<0>(targetIndices_min_rusult[i]);
                const std::shared_ptr<ReflectorPanelPoint>& secondPoint = std::get<1>(targetIndices_min_rusult[i]);
                //  if(i == polygon.size() - 1){
                //    std::cout << secondPoint->id << std::endl;
                // }else{
                std::cout <<"("<< firstPoint->id <<":"<< secondPoint->id<<") "<< std::endl;
                // }
            }




        }else if(!isGetResultMapvalueCallbackfig_||!isPublish_matchcostmapwithscan_mssage_ ){


            //scan_CB_new1 经典三边定位最小二乘法 每次计算得到的 mapTbase_link 的j结束时间
            std::chrono::time_point<std::chrono::high_resolution_clock> trilateration_mapTbase_link_pose_endTime= std::chrono::high_resolution_clock::now();
            // 计算并转换为毫秒
            auto trilateration_ms = std::chrono::duration_cast<std::chrono::milliseconds>(trilateration_mapTbase_link_pose_endTime - trilateration_mapTbase_link_pose_startTime_);
            // auto trilateration_s = std::chrono::duration_cast<std::chrono::seconds>(trilateration_ms);//转成秒
            double trilateration_s = static_cast<double>(trilateration_ms.count()) / 1000.0;//转成秒
            std::cout << "6. if(isNotMin_varianceNum_sourse_>isNotMin_varianceNum_target_) isNotMin_varianceNum_target_ " <<isNotMin_varianceNum_target_<< std::endl;
            if(


                    trilateration_s>targetTrilaterationTimeOut_
                    ){

                trilateration_mapTbase_link_pose_startTime_=std::chrono::high_resolution_clock::now();

                firstTrilateration_with_yawFig_=false;


                //scan_CB_new1
                isTrilateration_with_yawFig_=true;
                firstTrilateration_with_yawFig_=false;

                if(time_log_fig_==13) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                    std::string message = "未计算出有效的定位数据导致超时，当前累计时间：" + std::to_string(trilateration_s) + " s， 超时阀值 ：" + std::to_string(targetTrilaterationTimeOut_)+" s";
                    // 输出执行时间
                    std::cout <<message << std::endl;
                    resultChineseMessage =message+ "[计算定位数据数据超时]";
                    resultENMessage = "Failure to calculate valid positioning data resulted in timeout, current cumulative time" + std::to_string(trilateration_s) + " s， Timeout threshold ：" + std::to_string(targetTrilaterationTimeOut_)+" s";
                    className = __FILE__; // 更改为实际的文件名
                    classLine = __LINE__; // 更改为实际的行号
                    geometry_msgs::Pose mapTbase_link;
                    double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                    // /trilateration_time_log
                    publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
                }
            }

            std::cout << "检测 三边定位最小二乘法计算得到的mapTbase_link结果不符合阀值条件的设定的目标超时时间： targetTrilaterationTimeOut_ " <<targetTrilaterationTimeOut_<< " ,当前的source时间 trilateration_s: "<<trilateration_s<<std::endl;


        }
        else{//todo 当前 baselink对应的  result_Mapvalue_， result_Mapvalue： 直接发得到的baselink对应的值
            evaluateTrilaterationTimeout();

            //=======================================================================
        }//else if (isGetResultMapvalueCallbackfig_&&result_Mapvalue_!=free_cell_)


        //scan_CB_new1 缓存上一次的数据
        if(map_targetIndices_rusults.size()==1){
            mapTbase_link_old_trilateration_=mapTbase_link_trilateration_new_;
            std::cout << "7. if(map_targetIndices_rusults.size()==1){  " << std::endl;

        }else{
            mapTbase_link_old_trilateration_=mapTbase_transform_min;
            std::cout << "8. if(map_targetIndices_rusults.size()==1){  " << std::endl;

        }


        if(isReadMatchokReflectorPanel_){

            resultENMessage = "Matching algorithm: breadth first algorithm for matching+prioritizing the use of well matched reflector data [MatchokReflectorPanelPoint]";
            className = __FILE__; // 更改为实际的文件名
            classLine = __LINE__; // 更改为实际的行号

        }else{
            std::string message = "反光柱匹配算法： 只用匹配走广度优先特征匹配算法单匹配模式 =》 需要匹配的边长数量：" + std::to_string(targetEdges.size()) + "  条， 次的执行时间 ： " + std::to_string(duration4.count())+" milliseconds";
            std::cout <<message<< std::endl;
            // 输出执行时间
            std::cout <<message << std::endl;
            resultChineseMessage =message+ "[匹配数据]";
            resultENMessage = "Reflective column matching algorithm: using only the breadth first matching algorithm [MatchokReflectorPanelPoint]";
            className = __FILE__; // 更改为实际的文件名
            classLine = __LINE__; // 更改为实际的行号


        }


        if(time_log_fig_==0||time_log_fig_==5) {//-1:全部关闭，0：全部打开 ，1：打开第一个，2：打开第二个 3：.....
            geometry_msgs::Pose mapTbase_link;
            double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
            // indices.size();  matchokReflectorPanelPointEnd_Unordered_map_.size()
            publishTrilaterationLog(0, duration4.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);


        }
        //------------------------------------------------------------------------
        //scan_CB_new1 6：匹配算法：从查找圆心到匹配完成需要的总执行时间
        // 计算并转换为毫秒
        auto duration5 = std::chrono::duration_cast<std::chrono::milliseconds>(end4 - start1);
        if(time_log_fig_==-2|| time_log_fig_==0||time_log_fig_==6) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

            std::string message = "反光柱匹配算法：只用广度优先特征匹配算法  需要匹配的边长数量：" + std::to_string(targetEdges.size()) + " 条， 从查找圆心到匹配完成需要的总执行时间 ：" + std::to_string(duration5.count())+" milliseconds";

            if(isReadMatchokReflectorPanel_){
                message = " 5678 存在上一次的匹配数据时优先使用匹配好的反光板数据 + 匹配走广度优先特征匹配算法混合匹配模式 =》 需要匹配的边长数量：" + std::to_string(targetEdges.size()) + "  条， 从查找圆心到匹配完成需要的总执行时间 ： " + std::to_string(duration4.count())+" milliseconds";
            }

            std::cout <<message<< std::endl;
            // 输出执行时间
            std::cout <<message << std::endl;
            resultChineseMessage =message+ "[匹配数据]";
            resultENMessage = "Reflective Column Matching Algorithm [MatchokReflectorPanelPoint]";
            className = __FILE__; // 更改为实际的文件名
            classLine = __LINE__; // 更改为实际的行号
            geometry_msgs::Pose mapTbase_link;
            double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
            // /trilateration_time_log
            publishTrilaterationLog(0, duration5.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);

        }


        //scan_CB_new1 只显示一次
        // if(isFirtFanguangbanShowAllMaker_&&indices_first.size()>0&&reflectorPanelPointAllMaps_.size()>0){
        if(isFirtFanguangbanShowAllMaker_&&reflectorPanelPointAllMaps_.size()>0){
            isFirtFanguangbanShowAllMaker_= false;
            // 显示所有的反光柱
            fanguangbanShowAllMaker(reflectorPanelPointAllMaps_);
        }

    } catch (tf::TransformException& ex) {
        ROS_WARN_THROTTLE(1.0, "Failed to get odom transformation: %s", ex.what());
//        previous_base_link_to_odom_transform_=base_link_to_odom_transform_;
//        previous_base_link_to_map_transform_ =base_link_to_map_transform_;
//        std::this_thread::sleep_for(std::chrono::milliseconds(200));
        return false;
    }
    // scan_CB_new1
    return true;
}
//订阅雷达数,未用卡尔曼
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scanMsg) {

    // 创建一个线程并分离它，使其自动销毁
    std::thread([scanMsg]() {
        //  scan_CB(scanMsg);
        scan_CB_new1(scanMsg);
    }).detach();


}

//通过一个反光柱的数据计算小车定位坐标 (使用反光柱的全局坐标计算)
bool calculateBaseLinkToMapTransform(
        const std::shared_ptr<ReflectorPanelPoint>& reflectorPanelPoint,
        tf::TransformListener& tf_listener_,
        tf::Transform& base_link_to_map_transform)
{


    if(reflectorPanelPoint->isMatchok_!=1)
        return false;

    std::cout <<"8. 算法定位： 之前已经配准过的数据支持使用一个反光板计算小车定位坐标，使用坐标变化来做 processQueue 5136" << std::endl;

    //===============================================================
    tf::Transform laserTscanPoint_transform;
    laserTscanPoint_transform.setOrigin(tf::Vector3(reflectorPanelPoint->forecast_laserTscan_x, reflectorPanelPoint->forecast_laserTscan_y, reflectorPanelPoint->forecast_laserTscan_z));
    //tf::Quaternion q;
    tf::Quaternion q(reflectorPanelPoint->forecast_laserTscan_rx, reflectorPanelPoint->forecast_laserTscan_ry, reflectorPanelPoint->forecast_laserTscan_rz, reflectorPanelPoint->forecast_laserTscan_rw); //  x, y, z，w, 分量
    // q.setRPY(0, 0, reflectorPanelPoint->laserTscan_yaw);
    laserTscanPoint_transform.setRotation(q);

    std::cout << "==============================laserTscanPoint_transform===================================" << std::endl;
    std::cout << "laserTscanPoint_transform " << std::endl;



    // 打印平移信息
    std::cout << "打印平移信息  Translation: ("
              << laserTscanPoint_transform.getOrigin().getX() << ", "
              << laserTscanPoint_transform.getOrigin().getY() << ", "
              << laserTscanPoint_transform.getOrigin().getZ() << ")" << std::endl;

    // 打印旋转信息（四元数）
    std::cout << "打印旋转信息  Rotation (Quaternion): ("
              << laserTscanPoint_transform.getRotation().getX() << ", "
              << laserTscanPoint_transform.getRotation().getY() << ", "
              << laserTscanPoint_transform.getRotation().getZ() << ", "
              << laserTscanPoint_transform.getRotation().getW() << ")" << std::endl;


    //  tf::Transform mapTscanPoint_transform1=base_link_to_map_transform_ * laser_to_base_link_transform_*laserTscanPoint_transform;


    //todo mapTscanPoint
    tf::Transform mapTscanPoint_transform;
    //   mapTscanPoint = mapTodom * odomTbase_link * base_linkTlaser * laserTscanPoint
    mapTscanPoint_transform.setOrigin(tf::Vector3(reflectorPanelPoint->x, reflectorPanelPoint->y, reflectorPanelPoint->z));
    //tf::Quaternion q;
    tf::Quaternion q1(reflectorPanelPoint->rx, reflectorPanelPoint->ry, reflectorPanelPoint->rz, reflectorPanelPoint->rw); //  x, y, z，w, 分量
    // q.setRPY(0, 0, reflectorPanelPoint->laserTscan_yaw);
    mapTscanPoint_transform.setRotation(q1);


    std::cout << "============================== mapTscanPoint_transform ===================================" << std::endl;
    std::cout << "mapTscanPoint_transform " << std::endl;

    // 打印平移信息
    std::cout << "打印平移信息  Translation: ("
              << mapTscanPoint_transform.getOrigin().getX() << ", "
              << mapTscanPoint_transform.getOrigin().getY() << ", "
              << mapTscanPoint_transform.getOrigin().getZ() << ")" << std::endl;

    // 打印旋转信息（四元数）
    std::cout << "打印旋转信息  Rotation (Quaternion): ("
              << mapTscanPoint_transform.getRotation().getX() << ", "
              << mapTscanPoint_transform.getRotation().getY() << ", "
              << mapTscanPoint_transform.getRotation().getZ() << ", "
              << mapTscanPoint_transform.getRotation().getW() << ")" << std::endl;




    //求 mapTbaselink  = mapTscanPoint * scanPointTlaser * laserTbase_link
    base_link_to_map_transform  = mapTscanPoint_transform *laserTscanPoint_transform.inverse()*laser_to_base_link_transform_.inverse() ;

    //mapTscanPoint = mapTbaselink * baselinkTlaser * laserTscanPoint
    // mapTscanPoint_transform=base_link_to_map_transform*laser_to_base_link_transform*laserTscanPoint_transform;

    return true;


}






bool isTransformOutOfRange(const tf::Transform& current_transform, const tf::Transform& previous_transform, double pos_threshold, double rot_threshold) {
    // 计算位置差
    double dx = current_transform.getOrigin().getX() - previous_transform.getOrigin().getX();
    double dy = current_transform.getOrigin().getY() - previous_transform.getOrigin().getY();
    double dz = current_transform.getOrigin().getZ() - previous_transform.getOrigin().getZ();
    double distance = std::sqrt(dx * dx + dy * dy + dz * dz);

    // 计算角度差
    tf::Quaternion q_diff = current_transform.getRotation() * previous_transform.getRotation().inverse();
    double angle = 2 * std::acos(q_diff.getW());

    // 检查是否超出阈值
    return (distance > pos_threshold) || (angle > rot_threshold);
}




void processQueueSub() {

    // static tf::TransformBroadcaster tf_broadcaster_;
    while (iswileTure_) {
        if(!iswileTure_){
            std::cout<<"1.算法定位：processQueue： 经典三边定位最小二乘法 iswileTure_"<< "false" <<std::endl;
            return;
        }

        ros::spinOnce();//防止ros通讯堵塞


        if(!is_trilateration_){
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            continue;

        }


        //开始时间
        auto start = std::chrono::high_resolution_clock::now();
        {//代码块





            std::unique_lock<std::mutex> lock(queue_mutex_, std::try_to_lock);
            if (!lock.owns_lock()) {

                if(!isUsingThirdPartyLocalization_){
                    double odom_to_map_x= tf_last_odom_to_map_transform_.getOrigin().getX();
                    double odom_to_map_y= tf_last_odom_to_map_transform_.getOrigin().getY();
                    if(fabs(odom_to_map_x)+fabs(odom_to_map_y)<0.0001) {
                        std::this_thread::sleep_for(std::chrono::milliseconds(10));
                        continue;
                    }

                    // todo 1. isSendTf_
                    if(isSendTf_|| test_num_!=1){
                        tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));

                        // 匹配完成时间
                        auto end = std::chrono::high_resolution_clock::now();
                        // 计算并转换为毫秒
                        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                        // 输出执行时间
                        std::cout <<"使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标。[mutex]， "<< "   执行时间：" << duration.count() << std::endl;

                        //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                        if(time_log_fig_==0||time_log_fig_==10) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                            std::string resultType = "trilateration_execution";
                            std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标。[mutex]";
                            std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot. [mutex]";
                            std::string className = __FILE__; // 更改为实际的文件名
                            int classLine = __LINE__; // 更改为实际的行号
                            string matchReflectorPaneIds="EMPTY";
                            geometry_msgs::Pose mapTbase_link;
                            double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                            ///trilateration_time_log
                            publishTrilaterationLog(0, duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
                        }
                    }//if(isSendTf_)

                }else{
                    tf_last_odom_to_map_transform_ =odom_to_map_transform_;
                }

                // 如果锁不可用，直接跳过本次循环
                std::cout<<"4.processQueue 如果锁不可用，直接跳过本次循环"<<std::endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;
            }


            //===========================================================================================
            //todo 通过里程计计算位置 记录日志使用
            tf::Transform mapTbase_link_transformlog =    tf_last_odom_to_map_transform_*base_link_to_odom_transform_;
            geometry_msgs::Pose mapTbase_linklog;
            // 设置位置
            mapTbase_linklog.position.x = mapTbase_link_transformlog.getOrigin().getX();
            mapTbase_linklog.position.y = mapTbase_link_transformlog.getOrigin().getY();
            mapTbase_linklog.position.z = mapTbase_link_transformlog.getOrigin().getZ();

            // 设置方向（四元数）
            // 假设这是一个指向Z轴正方向的单位四元数，表示没有旋转
            mapTbase_linklog.orientation.x = mapTbase_link_transformlog.getRotation().getX();
            mapTbase_linklog.orientation.y = mapTbase_link_transformlog.getRotation().getY();
            mapTbase_linklog.orientation.z = mapTbase_link_transformlog.getRotation().getZ();
            mapTbase_linklog.orientation.w = mapTbase_link_transformlog.getRotation().getW();
            //  tf::Quaternion q11(mapTbase_linklog.orientation.x, mapTbase_linklog.orientation.y, mapTbase_linklog.orientation.z, mapTbase_linklog.orientation.w); //  x, y, z，w, 分量
            //  double reflectorPanelPoints_laserTscan_yaw1   = tf::getYaw(q11);
            double mapTbase_link_yaw_log=tf::getYaw(mapTbase_linklog.orientation); // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
            //===========================================================================================

            //todo  1. 使用上一次 mapTodom的数据，继续使用上一次的tf数据。用里程计做定位变化
            if ( matchokReflectorPanelPoint_.empty()) { //

                //是否配合第三方定位算法一起使用，比如 amcl等
                if(!isUsingThirdPartyLocalization_){

                    // todo 2. isSendTf_
                    if(isSendTf_|| test_num_!=2)
                        tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));

                    // 匹配完成时间
                    auto end = std::chrono::high_resolution_clock::now();
                    // 计算并转换为毫秒
                    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                    // 输出执行时间
                    //  std::cout <<"使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [队列 matchokReflectorPanelPoint_.empty()] "<< "   执行时间：" << duration.count() << std::endl;

                    //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                    if(time_log_fig_==0||time_log_fig_==10) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                        std::string resultType = "trilateration_execution";
                        std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [队列 matchokReflectorPanelPoint_.empty()]";
                        std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot.[queue matchokReflectorPanelPoint_.empty()]";
                        std::string className = __FILE__; // 更改为实际的文件名
                        int classLine = __LINE__; // 更改为实际的行号
                        string matchReflectorPaneIds="EMPTY";

                        ///trilateration_time_log
                        publishTrilaterationLog(0, duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size() ,matchReflectorPaneIds,mapTbase_linklog,mapTbase_link_yaw_log);
                    }
                }else{
                    tf_last_odom_to_map_transform_ =odom_to_map_transform_;

                }

                // std::cout <<"TransformException  发布 tf " << std::endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                //  lock.unlock();
                continue;
            }
            // 处理队列中的数据 841
            auto reflectorPanelPoints = matchokReflectorPanelPoint_.front();
            matchokReflectorPanelPoint_.pop();

            if(reflectorPanelPoints.size()==0){//数据为空

                //是否配合第三方定位算法一起使用，比如 amcl等
                if(!isUsingThirdPartyLocalization_){
                    double odom_to_map_x= tf_last_odom_to_map_transform_.getOrigin().getX();
                    double odom_to_map_y= tf_last_odom_to_map_transform_.getOrigin().getY();
                    if(fabs(odom_to_map_x)+fabs(odom_to_map_y)<0.001)
                    {
                        std::this_thread::sleep_for(std::chrono::milliseconds(10));
                        continue;
                    }
                    // todo 3. isSendTf_
                    if(isSendTf_|| test_num_!=3)
                        tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));

                    // 匹配完成时间
                    auto end = std::chrono::high_resolution_clock::now();
                    // 计算并转换为毫秒
                    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                    // 输出执行时间
                    //  std::cout <<"算法定位： processQueue： 基于之前已经匹配上了反光板的 算法定位， "<< "   执行时间：" << duration.count() << std::endl;
                    //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                    if(time_log_fig_==0||time_log_fig_==10) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                        std::string resultType = "trilateration_execution";
                        std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [队列 matchokReflectorPanelPoint->reflectorPanelPoints.size()==0]";
                        std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot.[队列 matchokReflectorPanelPoint->reflectorPanelPoints.size()==0]";
                        std::string className = __FILE__; // 更改为实际的文件名
                        int classLine = __LINE__; // 更改为实际的行号
                        string matchReflectorPaneIds="EMPTY";
                        ///trilateration_time_log
                        publishTrilaterationLog(reflectorPanelPoints.size(), duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size() ,matchReflectorPaneIds,mapTbase_linklog,mapTbase_link_yaw_log);

                    }


                }//if(!isUsingThirdPartyLocalization_)
                else{
                    tf_last_odom_to_map_transform_ =odom_to_map_transform_;

                }

                // std::cout <<"TransformException  发布 tf " << std::endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;
            }

            ///reflectorPanelPoints.size()
            //===========================================================================================
            //todo 匹配上的id 记录日志使用
            string matchReflectorPaneIds_log="EMPTY";
            for(int i=0;i<reflectorPanelPoints.size();i++){
                if(i==0)
                    matchReflectorPaneIds_log=reflectorPanelPoints[i]->id;
                else
                    matchReflectorPaneIds_log=matchReflectorPaneIds_log+","+reflectorPanelPoints[i]->id;
            }
            //===========================================================================================


            //todo 2. 之前已经有配准过的数据支持使用一个反光板计算小车定位坐标，使用坐标变化来做

            if((reflectorPanelPoints.size()>0&& reflectorPanelPoints.size()<=2&&firstTrilateration_with_yawFig_)||test_num_==13){

                //是否配合第三方定位算法一起使用，比如 amcl等
                if(!isUsingThirdPartyLocalization_){
                    double odom_to_map_x= tf_last_odom_to_map_transform_.getOrigin().getX();
                    double odom_to_map_y= tf_last_odom_to_map_transform_.getOrigin().getY();
                    if(fabs(odom_to_map_x)+fabs(odom_to_map_y)<0.001)
                    {
                        std::this_thread::sleep_for(std::chrono::milliseconds(10));
                        continue;
                    }

                    // todo 6. isSendTf_
                    if(isSendTf_|| test_num_!=6)
                        tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));


                    // 匹配完成时间
                    auto end = std::chrono::high_resolution_clock::now();
                    // 计算并转换为毫秒
                    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                    // 输出执行时间
                    //  std::cout <<"算法定位： processQueue： 基于之前已经匹配上了反光板的 算法定位， "<< "   执行时间：" << duration.count() << std::endl;
                    //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                    if(time_log_fig_==0||time_log_fig_==10) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                        std::string resultType = "trilateration_execution";
                        std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [matchokReflectorPanelPointEnd_Unordered_map_.size()==0,calculateBaseLinkToMapTransform2 函数反回 false ]";
                        std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot.[matchokReflectorPanelPointEnd_Unordered_map_.size()==0,calculateBaseLinkToMapTransform2 -> fasle ]";
                        std::string className = __FILE__; // 更改为实际的文件名
                        int classLine = __LINE__; // 更改为实际的行号
                        string matchReflectorPaneIds=matchReflectorPaneIds_log;
                        ///trilateration_time_log
                        publishTrilaterationLog(reflectorPanelPoints.size(), duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_linklog,mapTbase_link_yaw_log);
                    }

                }//if(!isUsingThirdPartyLocalization_)
                else{
                    tf_last_odom_to_map_transform_ =odom_to_map_transform_;

                }
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;


                //  }else if(reflectorPanelPoints.size()>2){
            }else if(reflectorPanelPoints.size()>findPolygonIndices_threshold_){

                // std::cout <<"---------------------------经典三边定位最小二乘法-------------------------------"  <<std::endl;
                //  std::cout <<" 6583  bbbbbbbbbbbbbbbbbbbb matchokReflectorPanelPointEnd_Unordered_map_.size()， "<< matchokReflectorPanelPointEnd_Unordered_map_.size() << std::endl;

//                     if(reflectorPanelPoints.size()<2)
//                         continue;
                //todo 3. 经典三边定位最小二乘法
                Pose   mapTbase_link_pose ;//经典三边定位最小二乘法 得到的移动机器人在map中的位置
                try{

                    // (法一，用向量计算base_link姿态),保证 reflectorPanelPoints.size()>2,否则内存报错 具体看trilateration_with_yaw_new
                    mapTbase_link_pose   =   trilateration_with_yaw_new(reflectorPanelPoints);//todo 此时的数据是旧的数据，应为里程计在时时变化，还需要更新里程计数据，6692

                    // mapTbase_link_pose   =   trilateration_with_yaw(reflectorPanelPoints);
                }catch (std::exception &e) {
                    cout<<"6607 error 经典三边定位最小二乘法: "<<e.what()<<endl;
                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    continue;
                }

                // (法二，用移动机器人的2点确定一条直线计算base_link姿态)
                // Pose   mapTbase_link_pose   =   trilateration_with_yaw(reflectorPanelPoints);

                //  std::cout <<" 2  bbbbbbbbbbbbbbbbbbbb matchokReflectorPanelPointEnd_Unordered_map_.size()， "<< matchokReflectorPanelPointEnd_Unordered_map_.size() << std::endl;


                if(fabs(mapTbase_link_pose.position.x)+fabs(mapTbase_link_pose.position.y)<0.00000001){
                    //  std::cout <<" 333 matchokReflectorPanelPointEnd_Unordered_map_.size()， "<< matchokReflectorPanelPointEnd_Unordered_map_.size() << std::endl;



                    // todo 9. isSendTf_
                    if(isSendTf_|| test_num_!=9) // 时时计算得到的移动机器人坐标 :mapTcurrentCalculateBaseLinkPose_frame_id_  tf_last_odom_to_map_transform_  mapTodom_transform
                        tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));


                    // 匹配完成时间
                    auto end = std::chrono::high_resolution_clock::now();
                    // 计算并转换为毫秒
                    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                    // 输出执行时间
                    //   std::cout <<"使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [trilateration_with_yaw_new 数据 mapTbase_link_pose 返回 0. "<< "   执行时间：" << duration.count() << std::endl;
                    //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                    if(time_log_fig_==0||time_log_fig_==10) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                        std::string resultType = "trilateration_execution";
                        std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标 [trilateration_with_yaw_new 数据 mapTbase_link_pose 返回 0 ]";
                        std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot.[trilateration_with_yaw_new return mapTbase_link_pose = 0]";
                        std::string className = __FILE__; // 更改为实际的文件名
                        int classLine = __LINE__; // 更改为实际的行号
                        string matchReflectorPaneIds=matchReflectorPaneIds_log;
                        ///trilateration_time_log
                        publishTrilaterationLog(reflectorPanelPoints.size(), duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_linklog,mapTbase_link_yaw_log);
                    }

                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    continue;
                }




                //通过pose
                Point point  =  mapTbase_link_pose.position;//mapTbase_link

                // todo mapTbase_link 经典三边定位最小二乘法 计算得到的 mapTbase_link
                tf::Transform mapTbase_link_trilateration;
                mapTbase_link_trilateration.setOrigin(tf::Vector3(point.x, point.y, 0.0));
                tf::Quaternion q;
                q.setRPY(0, 0, point.yaw);
                //mapTbase_link
                mapTbase_link_trilateration.setRotation(q);



                //记录是否已经定位成功了一次，第一次定位成功就设置true
                if(firstTrilateration_with_yawFig_==false)
                    firstTrilateration_with_yawFig_=true;


                //================================================================================================

                //   mapTbase_link_trilateration*
                //    mapTbase_link_trilateration =odom_to_map_transform_ * laser_odomTbaselint_transform;

                //   /**
                // 获取旧的 odomTbaselink的数据。  最新的移动机器人位置  因为里程计在时时变化，所以要更新里程计数据
                tf::StampedTransform laser_to_odom_transform  =  reflectorPanelPoints[0]->last_laser_to_odom_transform_;// laser_to_base_link_transform_

                tf::Transform     laser_odomTbaselint_transform =  laser_to_odom_transform * laser_to_base_link_transform_.inverse();

                tf::Transform     laser_mapTodom_transform =  mapTbase_link_trilateration*laser_odomTbaselint_transform.inverse();

                //todo 最新的移动机器人位置  因为里程计在时时变化，所以要更新里程计数据，重新赋值回 mapTbase_link_trilateration
                mapTbase_link_trilateration = laser_mapTodom_transform * base_link_to_odom_transform_;
                //  */

                //todo 发布 mapTbase_link 用经典三边定位最小二乘法计算的定位 【去对比当前的定位是否存在问题】
                tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_link_trilateration, ros::Time::now(), map_frame_id_, mapTcurrentCalculate_sanbian_BaseLinkPose_frame_id_));


                //是否配合第三方定位算法一起使用，比如 amcl等
                if(!isUsingThirdPartyLocalization_){

                    // todo 10. isSendTf_
                    if(isSendTf_|| test_num_!=10){//发布坐标关系

                        //todo 发布 mapTbase_link 用经典三边定位最小二乘法计算的定位 【去对比当前的定位是否存在问题】
                        // tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_link_trilateration, ros::Time::now(), map_frame_id_, base_link_frame_id_));

                        //=====================================================
                        // todo 计算  odom 在 map 坐标系下的变换 mapTodom
                        // tf::Transform map_to_odom_transform = base_link_to_map_transform * odom_to_base_link_transform.inverse();
                        // mapTodom  = mapTbase_link * base_linkTodom  , mapTbase_link =  mapTodom * odomTbase_link

                        //todo 发布 map 到 odom 的变换 mapTodom_transform
                        // tf_broadcaster_new_->sendTransform(tf::StampedTransform(laser_mapTodom_transform, ros::Time::now(), map_frame_id_, odom_frame_id_));


                        //   tf::Transform mapTodom_transform = mapTbase_link_trilateration * base_link_to_odom_transform_.inverse();//laser_mapTodom_transform

                        tf::Transform mapTodom_transform = laser_mapTodom_transform;

                        //                                                                 mapTodom_transform
                        tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTodom_transform, ros::Time::now(), map_frame_id_, odom_frame_id_));

                        //todo 最后一次定位数据 tf_last_odom_to_map_transform_=mapTodom_transform
                        tf_last_odom_to_map_transform_=mapTodom_transform;//laser_mapTodom_transform

                    }//if(isSendTf_)
                }//    if(!isUsingThirdPartyLocalization_)
                else{//重定位

                    ispublishInitialPosefig_ = true;//是否操作的重定位
                    //todo 发布 mapTbase_link 用经典三边定位最小二乘法计算的定位 【去对比当前的定位是否存在问题】
                    tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_link_trilateration, ros::Time::now(), map_frame_id_, mapTcurrentCalculate_sanbian_BaseLinkPose_frame_id_));

                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                    publishInitialPose(mapTbase_link_trilateration, map_frame_id_,initial_pose_pub_);

                    //    tf_last_odom_to_map_transform_ =odom_to_map_transform_;



                }//重定位


                // 匹配完成时间
                auto end = std::chrono::high_resolution_clock::now();
                // 计算并转换为毫秒
                auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                // 输出执行时间
                //  std::cout <<"算法定位：processQueue： 经典三边定位最小二乘法， "<< " 执行时间：" << duration.count() << std::endl;
                //发布性能评估数据 //   8.经典三边定位最小二乘法，推算位姿
                if(time_log_fig_==0||time_log_fig_==30) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                    std::string resultType = "trilateration_execution";
                    std::string resultChineseMessage = "经典三边定位最小二乘法计算移动机器人的位置坐标 [ trilateration_with_yaw_new ]";
                    std::string resultENMessage = "The classic trilateration least squares method is used to calculate the position coordinates of mobile robots. [ trilateration_with_yaw_new ]";
                    std::string className = __FILE__; // 更改为实际的文件名
                    int classLine = __LINE__; // 更改为实际的行号
                    string matchReflectorPaneIds=matchReflectorPaneIds_log;
                    geometry_msgs::Pose mapTbase_link;
                    // 设置位置
                    mapTbase_link.position.x = mapTbase_link_pose.position.x;
                    mapTbase_link.position.y = mapTbase_link_pose.position.y;
                    mapTbase_link.position.z = mapTbase_link_pose.position.z;

                    tf::Quaternion mapTbase_link_q;
                    mapTbase_link_q.setRPY(0, 0, mapTbase_link_pose.position.yaw);
                    // 设置方向（四元数）
                    // 假设这是一个指向Z轴正方向的单位四元数，表示没有旋转
                    mapTbase_link.orientation.x = mapTbase_link_q.getX();
                    mapTbase_link.orientation.y = mapTbase_link_q.getY();
                    mapTbase_link.orientation.z = mapTbase_link_q.getZ();
                    mapTbase_link.orientation.w = mapTbase_link_q.getW();
//                        tf::Quaternion q1(reflectorPanelPoints[i]->laserTscan_rx, reflectorPanelPoints[i]->laserTscan_ry, reflectorPanelPoints[i]->laserTscan_rz, reflectorPanelPoints[i]->laserTscan_rw); //  x, y, z，w, 分量
//                        double reflectorPanelPoints_laserTscan_yaw   = tf::getYaw(q1);
                    double mapTbase_link_yaw=mapTbase_link_pose.position.yaw; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                    ///trilateration_time_log
                    publishTrilaterationLog(reflectorPanelPoints.size(), duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);

                }


            }//if(reflectorPanelPoints.size()>0&& reflectorPanelPoints.size()<=2){}  else if(reflectorPanelPoints.size()>findPolygonIndices_threshold_)

            else{
                if(!isUsingThirdPartyLocalization_){
                    double odom_to_map_x= tf_last_odom_to_map_transform_.getOrigin().getX();
                    double odom_to_map_y= tf_last_odom_to_map_transform_.getOrigin().getY();
                    if(fabs(odom_to_map_x)+fabs(odom_to_map_y)<0.0001)
                    {
                        std::this_thread::sleep_for(std::chrono::milliseconds(50));
                        continue;
                    }

                    // todo 1. isSendTf_
                    if(isSendTf_|| test_num_!=1){
                        tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));

                        // 匹配完成时间
                        auto end = std::chrono::high_resolution_clock::now();
                        // 计算并转换为毫秒
                        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
                        // 输出执行时间
                        //  std::cout <<"使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标。[mutex]， "<< "   执行时间：" << duration.count() << std::endl;

                        //发布性能评估数据 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标
                        if(time_log_fig_==0||time_log_fig_==31) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                            std::string resultType = "trilateration_execution";
                            std::string resultChineseMessage = " 使用上一次 mapTodom -> tf_last_odom_to_map_transform_ 的tf数据，发布移动机器人的位置坐标。[mutex]";
                            std::string resultENMessage = "Use the previous mapTodom ->tf_1ast_odom_to-map_transform_ tf data to publish the position coordinates of the mobile robot. [mutex]";
                            std::string className = __FILE__; // 更改为实际的文件名
                            int classLine = __LINE__; // 更改为实际的行号
                            string matchReflectorPaneIds="EMPTY";
                            geometry_msgs::Pose mapTbase_link;
                            double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                            ///trilateration_time_log
                            publishTrilaterationLog(0, duration.count(), resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,matchokReflectorPanelPoint_.size(),matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
                        }
                    }//if(isSendTf_)

                }
                else{
                    tf_last_odom_to_map_transform_ =odom_to_map_transform_;

                }
            }
            // 休眠一段时间
            std::this_thread::sleep_for(std::chrono::milliseconds(2));
            // lock.unlock();

        }//end 代码块


        if(!iswileTure_){
            //  std::cout<<"2.算法定位：processQueue： 经典三边定位最小二乘法 iswileTure_"<< "false" <<std::endl;
            return;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }//while (true)
}
//去除三边计算代码，转入scan——callback计算
void processQueueSubNew() {

    // static tf::TransformBroadcaster tf_broadcaster_;
    //计算雷达点(已经换算到map下了)
    //  std::cout<<"start 111111111111111transformAndCalculate id: "<<id<<" 1111111111111多线程订阅tf数据11111111111 isMatchok_： "<<isMatchok_<<std::endl;
    while (iswileTure_) {
        if (!iswileTure_) {
            std::cout << "1.算法定位：processQueue： 经典三边定位最小二乘法 iswileTure_" << "false" << std::endl;
            return;
        }
        //todo sukai 测试
        if(!is_trilateration_){
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            continue;

        }

        //是否配合第三方定位算法一起使用，比如 amcl等
        if(!isUsingThirdPartyLocalization_){
            // todo 3. isSendTf_
            if(isSendTf_|| test_num_!=3)
                tf_broadcaster_new_->sendTransform(tf::StampedTransform(tf_last_odom_to_map_transform_, ros::Time::now(), map_frame_id_, odom_frame_id_));
        }//if(!isUsingThirdPartyLocalization_)
        else{
            tf_last_odom_to_map_transform_ =odom_to_map_transform_;

        }

        std::this_thread::sleep_for(std::chrono::milliseconds(50));

    }//    while (iswileTure_)

}



//开一个线程来 处理这匹配上的反光板数据,发布tf
void processQueue() {

    std::thread([]() {
        // processQueueSub();
        processQueueSubNew();
    }).detach();


}

void updatefunFanguangbanShowAllMaker() {
    std::cout<<"1.updatefunFanguangbanShowAllMaker  开一个线程"<<std::endl;
    // static   tf::TransformListener tf_listener_;
    while (iswileTure_) {
        if(!iswileTure_){
            return;
        }


        try {

            if(reflectorPanelPointAllMaps_.size()>0)
                fanguangbanShowAllMaker(reflectorPanelPointAllMaps_);


        } catch (tf::TransformException& ex) {
            ROS_ERROR("updatefunFanguangbanShowAllMaker  updatefunFanguangbanShowAllMaker  %s", ex.what());

        }

        ros::spinOnce();//防止ros通讯堵塞
        std::this_thread::sleep_for(std::chrono::milliseconds(5000));
    }
}


void updatefun() {
    std::cout<<"1.updatefun 开一个线程"<<std::endl;
    // static   tf::TransformListener tf_listener_;
    while (iswileTure_) {
        if(!iswileTure_){
            return;
        }


        // target_frame_id =map_frame_id_ ，source_frame_id= base_link_frame_id_  ,
        tf::StampedTransform base_link_to_map_transform;//mapTbase_link
        tf::StampedTransform laser_to_base_link_transform;//base_linkTlaser
        tf::StampedTransform base_link_to_odom_transform;//odomTbase_link
        tf::StampedTransform odom_to_map_transform;//mapTodom
        //todo laserTodom 获取 odom 到 laser 的变换  laserTodom
        tf::StampedTransform now_odomTlaser_transform;//laserTodom

        try {

            try {
                //todo base_linkTlaser
                if(fabs(laser_to_base_link_transform_.getOrigin().getX())+ fabs(laser_to_base_link_transform_.getOrigin().getX())>0.0000001){
                    laser_to_base_link_transform=laser_to_base_link_transform_;
                }else{

                    tf_listener_new_->waitForTransform(base_link_frame_id_, laser_frame_id_, ros::Time(), ros::Duration(1.0));
                    tf_listener_new_->lookupTransform(base_link_frame_id_, laser_frame_id_, ros::Time(0), laser_to_base_link_transform);
                    laser_to_base_link_transform_=laser_to_base_link_transform;
                }

            } catch (tf::TransformException& ex) {
                ROS_ERROR("base_linkTlaser ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

            }
            //--------------------------------------------------------

            try {
                //todo  mapTodom
                tf_listener_new_->waitForTransform(map_frame_id_, odom_frame_id_,ros::Time(0), ros::Duration(1.0));
                tf_listener_new_->lookupTransform(map_frame_id_, odom_frame_id_, ros::Time(0), odom_to_map_transform);
                odom_to_map_transform_=odom_to_map_transform;//mapTodom

                std::this_thread::sleep_for(std::chrono::milliseconds(5));

            } catch (tf::TransformException& ex) {
                ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

            }
            //--------------------------------------------------------

            try {

                //todo  odomTbase_link
                tf_listener_new_->waitForTransform(odom_frame_id_, base_link_frame_id_,ros::Time(0), ros::Duration(1.0));
                tf_listener_new_->lookupTransform(odom_frame_id_, base_link_frame_id_, ros::Time(0), base_link_to_odom_transform);
                base_link_to_odom_transform_=base_link_to_odom_transform;

                if(isfirstPrevious_base_link_to_odom_transform_){
                    isfirstPrevious_base_link_to_odom_transform_=false;
                    previous_base_link_to_odom_transform_=base_link_to_odom_transform;
                }
                std::this_thread::sleep_for(std::chrono::milliseconds(5));
            } catch (tf::TransformException& ex) {
                ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

            }
            //--------------------------------------------------------

            try {
                //todo  mapTbase_link
                tf_listener_new_->waitForTransform(map_frame_id_, base_link_frame_id_,ros::Time(0), ros::Duration(1.0));
                tf_listener_new_->lookupTransform(map_frame_id_, base_link_frame_id_, ros::Time(0), base_link_to_map_transform);
                base_link_to_map_transform_=base_link_to_map_transform;


                if(isfirstPrevious_base_link_to_map_transform_){
                    isfirstPrevious_base_link_to_map_transform_=false;
                    previous_base_link_to_map_transform_=base_link_to_map_transform;
                }

                std::this_thread::sleep_for(std::chrono::milliseconds(5));

            } catch (tf::TransformException& ex) {
                ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

            }
            //--------------------------------------------------------

            try {
                //todo laserTodom
                std::this_thread::sleep_for(std::chrono::milliseconds(5));
                tf_listener_new_->waitForTransform(odom_frame_id_, laser_frame_id_,ros::Time(0), ros::Duration(1.0));
                tf_listener_new_->lookupTransform(odom_frame_id_, laser_frame_id_, ros::Time(0), now_odomTlaser_transform);
                now_odomTlaser_transform_=now_odomTlaser_transform;

            } catch (tf::TransformException& ex) {
                ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

            }
            //--------------------------------------------------------


        }catch (tf::TransformException& ex) {
            ROS_ERROR("ReflectorPanelPoint::transformAndCalculate tf  %s", ex.what());

            continue;
        }
        //==========================================================


        try {
            //判断 costmap 是否有数据过来，超过时间没有数据认为，订阅不成功
            std::chrono::time_point<std::chrono::high_resolution_clock> matchcostmapwithscan_endTime= std::chrono::high_resolution_clock::now();
            // 计算并转换为毫秒
            auto tresultGetResultMapvalueCallback_ms = std::chrono::duration_cast<std::chrono::milliseconds>(matchcostmapwithscan_endTime - resultGetResultMapvalueCallback_startTime_);
            auto resultGetCalculateScanMapRatioCallback_ms = std::chrono::duration_cast<std::chrono::milliseconds>(matchcostmapwithscan_endTime - resultGetCalculateScanMapRatioCallback_startTime_);

            // auto trilateration_s = std::chrono::duration_cast<std::chrono::seconds>(trilateration_ms);//转成秒
            double tresultGetResultMapvalueCallback_s = static_cast<double>(tresultGetResultMapvalueCallback_ms.count()) / 1000.0;//转成秒
            double resultGetCalculateScanMapRatioCallback_s = static_cast<double>(resultGetCalculateScanMapRatioCallback_ms.count()) / 1000.0;//转成秒


            if(tresultGetResultMapvalueCallback_s>matchcostmapwithscanTimeOut_){
                isGetResultMapvalueCallbackfig_=false;
                result_Mapvalue_=-3;
            }else{
                isGetResultMapvalueCallbackfig_=true;
            }

            if(resultGetCalculateScanMapRatioCallback_s>matchcostmapwithscanTimeOut_){
                isGetCalculateScanMapRatioCallbackfig_=false;

                is_ratio_= false;//是否满足匹配条件
                matching_points_=0;
                scan_points_map_size_=0;
                total_points_size_=0;//雷达点数据量的有效数据量

            }else{
                isGetCalculateScanMapRatioCallbackfig_=true;
            }


        } catch (tf::TransformException& ex) {
            ROS_ERROR("resultGetResultMapvalueCallback  resultGetCalculateScanMapRatioCallback  %s", ex.what());

        }



        //不能接收自己发布的重定位消息
        if(ispublishInitialPosefig_){
            // 记录结束时间
            std::chrono::time_point<std::chrono::high_resolution_clock>  endTime = std::chrono::high_resolution_clock::now();
            // 计算并转换执行时间到秒（double类型）
            std::chrono::duration<double> duration = endTime - startTime_;
            double executionTimeInSeconds = duration.count();
            if(executionTimeInSeconds>3.0){
                ispublishInitialPosefig_=false;
                // 输出结果
                std::cout << " 不能接收自己发布的重定位消息 等待耗时: " << executionTimeInSeconds << " 秒" << std::endl;
            }
        }

        //todo 超过时间阀值，就认为反光板已经失去匹配关系。删除失去匹配关系的数据
        removeStaleReflectors();
        ros::spinOnce();//防止ros通讯堵塞
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
}

//心跳机制，检查校验m atch_costmap_with_scan_node 节点是否启动 , check_atch_costmap_with_scan_nodeStatusfun
void check_atch_costmap_with_scan_nodeStatusfun() {
    std::cout<<"1.check_atch_costmap_with_scan_nodeStatusfun 开一个线程"<<std::endl;
    // static   tf::TransformListener tf_listener_;
    while (iswileTure_) {
        if(!iswileTure_){
            return;
        }

        fanguangbantrilateration::matchcostmapwithscan_srvs matchcostmapwithscan_srv;
        matchcostmapwithscan_srv.request.request_type = "handleCheckNodeStart";  // 设置请求类型
        //表示等待5秒。如果服务在5秒内没有准备好，则会抛出错误。
//            if (!match_costmap_with_scan_service_client_.waitForExistence(ros::Duration(0.05))) {  // 5.0 等待5秒
//                ROS_ERROR("Service %s not available after waiting for 5 seconds", "match_costmap_with_scan_service");
//                return false;
//            }
        // 发送服务请求
        if (match_costmap_with_scan_service_client_.call(matchcostmapwithscan_srv))
        {

//
            if(time_log_fig_==21) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                std::string message = "心跳机制，检查校验 match_costmap_with_scan_node 节点是已经启动 ， check_atch_costmap_with_scan_nodeStatusfun";
                // 输出执行时间
                std::cout <<message << std::endl;
                std::string resultType = "check_atch_costmap_with_scan_nodeStatusfun";
                std::string resultChineseMessage =message+ "[心跳机制，检查校验 match_costmap_with_scan_node 节点是否启动]";
                std::string resultENMessage = "Heartbeat mechanism, check and verify that the match_comstmapw_ith_stcan_node node has already been started, check_atch_costmap_with_scan_nodeStatusfun";
                std::string className = __FILE__; // 更改为实际的文件名
                int  classLine = __LINE__; // 更改为实际的行号
                geometry_msgs::Pose mapTbase_link;
                double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                std::string matchReflectorPaneIds="";
                // /trilateration_time_log
                publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
            }
            std::cout <<GREEN<< " match_costmap_with_scan_node 节点工作正常 ..." <<RESET<< std::endl;

        }else{
            if(time_log_fig_==21) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

                std::string message = "心跳机制，检查校验 match_costmap_with_scan_node 节点未启动，  check_atch_costmap_with_scan_nodeStatusfun";
                std::string resultType = "check_atch_costmap_with_scan_nodeStatusfun";
                // 输出执行时间
                std::cout <<message << std::endl;
                std::string resultChineseMessage =message+ "[心跳机制，检查校验节点是否启动]";
                std::string resultENMessage = "Heartbeat mechanism, check and verify that the match_comstmapw_ith_stcan_node node is not started, check_atch_costmap_with_scan_nodeStatusfun";
                std::string className = __FILE__; // 更改为实际的文件名
                int classLine = __LINE__; // 更改为实际的行号
                std::string matchReflectorPaneIds="";
                geometry_msgs::Pose mapTbase_link;
                double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
                // /trilateration_time_log
                publishTrilaterationLog(0, 0, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_,0,matchokReflectorPanelPointEnd_Unordered_map_.size(),matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw);
            }

            std::cout <<RED<< " match_costmap_with_scan_node 节点工作异常或未启动，这可能会导致出现定位丢失问题..." <<RESET<< std::endl;
        }




        std::this_thread::sleep_for(std::chrono::milliseconds(5000));
    }
}
//=============================================================

// 函数用于清空文件中的所有数据
void clearFile( std::string& filename){
    // 加锁
    std::lock_guard<std::mutex> lock(fileMutex);
    std::lock_guard<std::mutex> lock1(filePointMutex);

    // 打开文件并清空内容
    std::ofstream ofs(filename, std::ios::out | std::ios::trunc);
    ofs.close();

    // 解锁
    // lock_guard 在作用域结束时自动解锁，所以此处不需要手动解锁

}

std::string transformToString(const tf::Transform& transform) {
    std::ostringstream oss;

    // 获取平移部分
    tf::Vector3 origin = transform.getOrigin();
    oss << "Translation: ["
        << origin.x() << ", "
        << origin.y() << ", "
        << origin.z() << "]";

    // 获取旋转部分（四元数）
    tf::Quaternion rotation = transform.getRotation();
    oss << ", Rotation (quaternion): ["
        << rotation.x() << ", "
        << rotation.y() << ", "
        << rotation.z() << ", "
        << rotation.w() << "]";

    return oss.str();
}
// 保存充电装坐标
bool  saveTransformToTxt(const tf::Transform& transform, const std::string& filename)
{
    try {
        std::ofstream outFile(filename);
        if (outFile.is_open()) {
            outFile << std::fixed << std::setprecision(4);//保留小数后4位
            // 获取平移数据
            tf::Vector3 origin = transform.getOrigin();
            outFile << origin.x() << "," << origin.y() << "," << origin.z() << ",";

            // 获取旋转数据
            tf::Quaternion rotation = transform.getRotation();
            outFile << rotation.x() << "," << rotation.y() << "," << rotation.z() << "," << rotation.w();

            outFile.close();
            std::cout << "Transform data saved successfully to " << filename << std::endl;

        } else {
            std::cerr << "Unable to open file for writing: " << filename << std::endl;
            return false;
        }

    }catch (std::exception e){
        std::cout<<"保存文件失败，exception。filename： "<<filename<<std::endl;
        return false;
    }
    return true;
}
//todo ================================================接收用户操作指令=====================================================================

//todo 保存当前位置
void handleSaveMapTbaselinkPose(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {


    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    //保存文件
    bool  saveTransformToTxtfig =   saveTransformToTxt( base_link_to_map_transform_, mapTbaselinkPose_path_);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    if(saveTransformToTxtfig){
        re.resultTxt=transformToString(base_link_to_map_transform_);
        re.result="ok";
        re.success=true;

    }else{
        re.result="error";
        re.success=false;

    }
}


//控制台打印已经用鼠标点击地图选择好的反光板
void handlePrintSavePosint(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {

    //std::map<std::string,Point> pointsSaveMap_;//或其鼠标点击rviz获取反光板坐标
    if(pointsSaveMap_.size()==0){
        std::cerr << "pointsSaveMap_ is empty" << std::endl;
        re.resultTxt = "pointsSaveMap_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    printPointsSaveMap();
    re.result_numPoints=pointsSaveMap_.size();
    re.result="ok";
    re.success=true;
}
//--------------------------------

//控制台打印所有已经转为ReflectorPanel反光板std::shared_ptr<ReflectorPanelPoint>对象的反光板数据
void handlePrintPosintAllMap(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    //std::map<std::string ,std::shared_ptr<ReflectorPanelPoint>> reflectorPanelPointAllMaps_;
    if(reflectorPanelPointAllMaps_.size()==0){
        std::cerr << "reflectorPanelPointAllMaps_ is empty" << std::endl;
        re.resultTxt = "reflectorPanelPointAllMaps_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }

    printPointAllMaps();
    re.result_numPoints=reflectorPanelPointAllMaps_.size();
    re.result="ok";
    re.success=true;
}

//todo std::vector 储存所有 排序好的反光板数据 排序规则为 反光板点位里小车中心坐标比较最近的在前面且x坐标是正值的数据。如果x坐标是负值说明反光板在小车的后面，理论上以经匹配过了不需要排在前面
//控制台打印所有已经转为ReflectorPanel反光板，并且排序好的反光板数据 std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> reflectorPanelPoints_vector_;
void handlePrintReflectorPanelPoints_vector(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {

    // std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> reflectorPanelPoints_vector_;
    if(reflectorPanelPoints_vector_.size()==0){
        std::cerr << "reflectorPanelPoints_vector_ is empty" << std::endl;
        re.resultTxt = "reflectorPanelPoints_vector_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    iswirtortSetPeriodically_=true;
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    std::stringstream ss;
    for (const auto& ptr : reflectorPanelPoints_vector_) {
        ss << "ID: " << ptr->id << ", Distance: " << ptr->distances << ", Position: (" << ptr->x << ", " << ptr->y << ")" << std::endl;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    iswirtortSetPeriodically_=false;

    // 打印到控制台
    std::cout << ss.str();
    // 赋值给 re.resultTxt
    re.resultTxt = ss.str();
    re.result_numPoints=reflectorPanelPoints_vector_.size();
    re.result="ok";
    re.success=true;
}

//控制台打印所有已经转为ReflectorPanel反光板，std::tuple<double, double> 为 key,map下的反光板全局坐标 std::tuple<x, y> 为key， std::unordered_map<std::tuple<double, double>, std::shared_ptr<ReflectorPanelPoint>, TupleHash, TupleEqual>  reflectorPanelPointAllUnordered_maps_;
void handlePrintReflectorPanelPointAllUnordered_maps(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {

    //std::unordered_map<std::tuple<double, double>, std::shared_ptr<ReflectorPanelPoint>, TupleHash, TupleEqual>  reflectorPanelPointAllUnordered_maps_;
    if(reflectorPanelPointAllUnordered_maps_.size()==0){
        std::cerr << "reflectorPanelPointAllUnordered_maps_ is empty" << std::endl;
        re.resultTxt = "reflectorPanelPointAllUnordered_maps_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    std::stringstream ss;

    for (const auto& entry : reflectorPanelPointAllUnordered_maps_) {
        const auto& key = entry.first;
        const auto& value = entry.second;
        ss << "Key: (" << std::get<0>(key) << ", " << std::get<1>(key) << ") - ID: " << value->id
           << ", Position: (" << value->x << ", " << value->y << ")" << std::endl;
    }
    // 打印到控制台
    std::cout << ss.str();
    // 赋值给 re.resultTxt
    re.resultTxt = ss.str();
    re.result_numPoints=reflectorPanelPointAllUnordered_maps_.size();
    re.result="ok";
    re.success=true;
}



void handlePrintPrintIndices(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {

    re.resultTxt = "function is empty" ;
    re.result="ok";
    re.success=true;
}

//--------------------------------------------------------------------------------------------------
//打印 计算好2点位带有边长数据的函数 std::unordered_map<std::vector<std::string>, std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>,VectorStringHash, VectorStringEqual> unorderedEdgeMapNew_IdKey_;
void handlePrintUnorderedEdgeMapNew_IdKey(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    //打印 计算好2点位带有边长数据的函数
    if(unorderedEdgeMapNew_IdKey_.size()==0){
        std::cerr << "unorderedEdgeMapNew_IdKey_ is empty" << std::endl;
        re.resultTxt = "unorderedEdgeMapNew_IdKey_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }

    //  2个key   std::vector<std::string> key = {"ID25", "ID9"};
    printUnorderedEdgeMapNew_IdKey(unorderedEdgeMapNew_IdKey_);
    re.result_numPoints=unorderedEdgeMapNew_IdKey_.size();
    re.result="ok";
    re.success=true;

}
//打印 计算好2点位带有边长数据的函数 std::unordered_map<std::string , std::vector<std::tuple<std::vector<double>, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>>> unorderedEdgeMapNew_;
void handlePrintUnorderedEdgeMapNew(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {

    if(unorderedEdgeMapNew_.size()==0){
        std::cerr << "unorderedEdgeMapNew_ is empty" << std::endl;
        re.resultTxt = "unorderedEdgeMapNew_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    //std::string 1个key ，储存所有有计算好边长的数据
    printUnorderedEdgeMapNew(unorderedEdgeMapNew_);
    re.result_numPoints=unorderedEdgeMapNew_.size();
    re.result="ok";
    re.success=true;

}

//打印 储存匹配上的点位，map下的反光板全局坐标 std::tuple<x, y> 为key std::unordered_map<std::tuple<double, double>, std::shared_ptr<ReflectorPanelPoint>, TupleHash, TupleEqual>  matchokReflectorPanelPointEnd_Unordered_map_;
void handlePrintMatchokReflectorPanelPointEnd_Unordered_map(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {





    if(matchokReflectorPanelPointEnd_Unordered_map_.size()==0){
        std::cerr << "matchokReflectorPanelPointEnd_Unordered_map_ is empty" << std::endl;
        re.resultTxt = "matchokReflectorPanelPointEnd_Unordered_map_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }


    while (iswileTure_){
        if(!iswileTure_){
            return;
        }
        if(!iswirtMatchokReflectoUnordered_map_){
            std::stringstream ss;
            for (const auto& entry : matchokReflectorPanelPointEnd_Unordered_map_) {
                const auto& key = entry.first;
                const auto& value = entry.second;
                //-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
                ss<<"isMatchok_ ： " <<(value->isMatchok_ == -1 ? "还未开始匹配" :(value->isMatchok_ == 2 ? "预测到匹配的点" :(value->isMatchok_ == 1 ? "匹配成功" :"匹配超时已经断开匹配")));
                ss <<"distances: "<<value->distances<<" ";
                ss << "Key: (" << std::get<0>(key) << ", " << std::get<1>(key) << ") - Value: ID=" << value->id << ", Position=(" << value->x << ", " << value->y << ")" << std::endl;
            }
            // 打印到控制台
            std::cout << ss.str();

            // 赋值给 re.resultTxt
            re.resultTxt = ss.str();
            re.result_numPoints=matchokReflectorPanelPointEnd_Unordered_map_.size();

            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            break;
        }
    }


    re.result="ok";
    re.success=true;

}
//-------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------------
//=======================================部署反光板的同时进行定位，尤其是长走廊环境必须这么做=============================================

// 实现删除单个鼠标获取rviz的缓存点 pointsSaveMap_ 位的逻辑
void handleDeletePosint(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Deleting a position..." << std::endl;

    if(req.request_posint_id.size()==0){
        std::cerr << "error Deleting a position..." <<"posint_id is empty" << std::endl;
        re.resultTxt = "posint_id is empty" ;
        re.result="error";
        re.success=false;
        return;
    }

    auto it = pointsSaveMap_.find(req.request_posint_id);
    if (it != pointsSaveMap_.end()) {
        pointsSaveMap_.erase(it);
        //  std::cout << "Key " << req.request_posint_id.x << " and its associated value have been removed.\n";
        re.result_numPoints=pointsSaveMap_.size();
        re.result="ok";
        re.success=true;
    } else {
        std::cerr << "Key not found in the map.\n";
        re.result_numPoints=pointsSaveMap_.size();
        re.result="error";
        re.success=false;
    }


}
// 实现删除所有缓存点位 pointsSaveMap_ 的逻辑，部署时鼠标获取的点。
void handleDeletePosintAll(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Deleting all positions..." << std::endl;
    pointsSaveMap_.clear();
    re.result_numPoints=pointsSaveMap_.size();
    re.result="ok";
    re.success=true;
}



//todo 依次储存每一个反光板在 map 下的坐标数据到 json文件中，并创建ReflectorPanelPoint对象
void handleSavePosint(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {

    std::cout << "====================================================================== " << std::endl;
    std::cout << "1.Saving positions..." << std::endl;

    if(pointsSaveMap_.size()==0){
        std::cerr << "error Saving positions..." <<"pointsSaveMap_ is empty" << std::endl;
        re.resultTxt = "pointsSaveMap_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }

//    if(req.request_shape.size()==0){
//        std::cerr << "error Saving positions..." <<"shape is empty" << std::endl;
//        re.resultTxt = "shape is empty" ;
//        re.result="error";
//        re.success=false;
//        return;
//    }

//    if(req.request_shape!="CYLINDER" ||req.request_shape!="CUBE"){
//        std::cerr << "error Saving positions..." <<"shape is invalid" << std::endl;
//        re.resultTxt = "shape is  invalid，Please enter CYLINDER OR CUBE " ;
//        re.result="error";
//        re.success=false;
//        return;
//    }

    //todo 0.0 不要清除反光板的在集合中的缓存数据，因为部署坐标时会多次增加数据，而不是重新开始部署数据，需要重新开始部署数据请先执行 handleDeletePosintAllWithSavePointFilePath 函数。
    //    //清除数据
    //    reflectorPanelPointAllMaps_.clear();
    //    reflectorPanelPoints_vector_.clear();
    //    reflectorPanelPointAllUnordered_maps_.clear();
    //    // 函数用于清空文件中的所有数据
    //    clearFile( savePointFilePath_);

    // 初始 反光板的数据，ReflectorPanelPoint 对象管理
    int marker_id=0;
    for (const auto& pair : pointsSaveMap_) {
        Point point=pair.second;

        auto  newPoint = ReflectorPanelPoint::createInstance2(point.id,marker_id, point.x, point.y, 0.0,0.0,0.0,0.0,1.0);
        newPoint->shape=req.request_shape;//储存反光板的形状
        newPoint->shape="CYLINDER";
        newPoint->reflectorPanelPoint_marker_id_=marker_id;
        newPoint->reflectorPanelPoint_text_id_=marker_id+1;
        reflectorPanelPointAllMaps_[newPoint->id]=newPoint;//map集合储存所有点位，

        //reflectorPanelPoints_vector_.insert(newPoint);//列表储存所有点位，通过tf订阅数据，实现自动排序功能
        addInsertNewPointToReflectorPanelPoints_vector(reflectorPanelPoints_vector_,newPoint);
        // 储存所有的点为 std::tuple<double, double> 为 key
        reflectorPanelPointAllUnordered_maps_[std::make_tuple(point.x, point.y)]=newPoint;
        marker_id=marker_id+2;
        //打印数据
        std::cout<<"打印数据: "<<newPoint<<std::endl;
        std::cout<<RED<<"=========================================================打印数据marker_id: "<<marker_id<<RESET<<std::endl;
    }
    std::cout << "pointsSaveMap_.size(): " <<pointsSaveMap_.size()<< std::endl;
    std::cout << "reflectorPanelPoints_vector_.size(): " <<reflectorPanelPoints_vector_.size()<< std::endl;

    clearFile( savePointFilePath_);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    //todo 写入json文件 储存反光板数据到json
    savePosintsWithJsonFile(reflectorPanelPoints_vector_);

    re.result_savepointnum=pointsSaveMap_.size();
    re.result="ok";
    re.success=true;
    std::cout << "ok Saving positions: " <<pointsSaveMap_.size()<< std::endl;
    pointsSaveMap_.clear();
    std::cout << "====================================================================== " << std::endl;

}


// 实现删除所有json文件中的 所有点位包括 缓存 pointAllMaps_
void handleDeletePosintAllWithSavePointFilePath(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Deleting all positions..." << std::endl;
    //清空map中的数据
    pointsSaveMap_.clear();
    reflectorPanelPointAllMaps_.clear();//先清空数据
    reflectorPanelPoints_vector_.clear();//先清空数据
    reflectorPanelPointAllUnordered_maps_.clear();//先清空数据
    // 函数用于清空文件中的所有数据
    clearFile( savePointFilePath_);
    re.result_numPoints=reflectorPanelPointAllMaps_.size();
    re.result="ok";
    re.success=true;
}
// 重新获取的JSON中的反光板单个点位
void handleReloadJSONPosintAll(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "reloadJSON all positions..." << savePointFilePath_<<std::endl;
    iswirtortSetPeriodically_=true;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    pointsSaveMap_.clear();
    reflectorPanelPoints_vector_.clear();
    reflectorPanelPointAllMaps_.clear();
    reflectorPanelPointAllUnordered_maps_.clear();
    //todo 读取json文件 加载文件中的json数据，读取反光板点为数据存入 dataInfoMaps_，loadAllPointJSONDataFromFile中包含了清除缓存数据reflectorPanelPointAllMaps.clear();reflectorPanelPoints_vector.clear();reflectorPanelPointAllUnordered_maps_.clear();
    loadAllPointJSONDataFromFile( savePointFilePath_,reflectorPanelPoints_vector_,reflectorPanelPointAllMaps_,reflectorPanelPointAllUnordered_maps_);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    iswirtortSetPeriodically_=false;
    re.result_numPoints=reflectorPanelPointAllMaps_.size();
    re.result="ok";
    re.success=true;

}


//删除单个json中的数据，删除单个数据后会全部重新计算特征数据
void handleDeletePosintWithJsonFile(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Deleting a position..." << std::endl;
    if(req.request_posint_id.size()==0){
        std::cerr << "error Deleting a position..." <<"posint_id is empty" << std::endl;
        re.resultTxt = "posint_id is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    is_trilateration_=false;//暂停定位


    string key =  req.request_posint_id; // 替换为你要删除的键

    //删除reflectorPanelPointAllMaps_的数据
    auto it = reflectorPanelPointAllMaps_.find(key);
    if (it != reflectorPanelPointAllMaps_.end()) {
        reflectorPanelPointAllMaps_.erase(it);
    }
    //删除数据 自动排序
    deleteNewPointToReflectorPanelPoints_vector(reflectorPanelPoints_vector_,key) ;

    //删除类ReflectorPanelPoint对象中的数据
    bool isDeleted = ReflectorPanelPoint::deleteInstance(key);
    if (!isDeleted) {
        std::cerr << "error Key " << key << " has been deleted.\n";
        re.result_numPoints=pointsSaveMap_.size();
        re.result="error";
        re.success=false;
        return;
    }




    pointsSaveMap_.clear();
    reflectorPanelPoints_vector_.clear();
    reflectorPanelPointAllUnordered_maps_.clear();
    // 函数用于清空文件中的所有数据
    clearFile( savePointFilePath_);
    // 遍历并复制数据
    for (const auto& entry : reflectorPanelPointAllMaps_) {
        const std::shared_ptr<ReflectorPanelPoint>& point = entry.second;
        if (point) {
            // 将数据添加到 multiset
            // reflectorPanelPoints_vector_.insert(point);
            addInsertNewPointToReflectorPanelPoints_vector(reflectorPanelPoints_vector_,point);
            // 将数据添加到 unordered_map
            reflectorPanelPointAllUnordered_maps_[{point->x, point->y}] = point;
        }
    }

    //todo 写入json文件 储存反光板数据到json
    savePosintsWithJsonFile(reflectorPanelPoints_vector_);

    //todo 3. 删除计算好的所有点位特征-json文件包括缓存
    unorderedEdgeMapNew_.clear();
    unorderedEdgeMapNew_IdKey_.clear();
    clearFile( calculateFilePath_);

    // todo 4. 计算所有点位特征

    // 实现计算所有点位特征并保存为json文件的逻辑
    if(reflectorPanelPoints_vector_.size()==0){
        std::cerr << "error Saving positions..." <<"reflectorPanelPoints_vector_ is empty" << std::endl;
        re.resultTxt = "reflectorPanelPoints_vector_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
    while (iswileTure_){
        if(!iswileTure_){
            return;
        }
        if(!iswirtMatchokReflectoUnordered_map_){
            //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
            matchokReflectorPanelPointEnd_Unordered_map_.clear();
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            break;
        }
    }

    //isMatchok_ -1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    ReflectorPanelPoint::initInstance();

    std::cout << "=================1=======================================.\n";
    // 计算2个点连成的 edge 直线边长数据 包含  saveJSONDataToFile 数据写入jion
    calculateEdge(reflectorPanelPoints_vector_,unorderedEdgeMapNew_,unorderedEdgeMapNew_IdKey_);
    is_trilateration_=true;
    std::cout << "=================2=========================================.\n";

    std::cout << "Key " << key << " has been removed from the JSON file.\n";
    re.result_numPoints=reflectorPanelPointAllMaps_.size();
    re.result="ok";
    re.success=true;

}

//更新单个json中的数据
void handleUpdatePosintWithJsonFile(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Deleting a position..." << std::endl;
    if(req.request_posint_id.size()==0|| fabs(req.request_posint_x)<=0.000001|| fabs(req.request_posint_y)<=0.000001){
        std::cerr << "error Update a position..." <<"posint_id is empty" << std::endl;
        re.resultTxt = "posint_id is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    is_trilateration_=false;//暂停定位
    string key =  req.request_posint_id; // 替换为你要删除的键
    //查找类ReflectorPanelPoint对象中的数据
    std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint;
    bool  getInstancefig = ReflectorPanelPoint::getInstance(key,reflectorPanelPoint);
    if(!getInstancefig){
        std::cerr << "error Update a position..." <<"reflectorPanelPoint is empty key: " <<key<< std::endl;
        re.resultTxt = "reflectorPanelPoint is empty: "+key ;
        re.result="error";
        re.success=false;
        is_trilateration_=true;
        return;
    }

//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    reflectorPanelPoint->isMatchok_=-1;
    reflectorPanelPoint->currentIsMatchok_=-1;
    reflectorPanelPoint->isAssignment_=-1;//1:不可以赋值 ，-1：可以赋值
    reflectorPanelPoint->x=req.request_posint_x;//更新数据应为是指针数据，所以reflectorPanelPointAllMaps_数据也会跟着改
    reflectorPanelPoint->y=req.request_posint_y;//更新数据应为是指针数据，所以reflectorPanelPointAllMaps_数据也会跟着改
    reflectorPanelPoint->isAssignment_=1;
    pointsSaveMap_.clear();

    // 函数用于清空文件中的所有数据
    clearFile( savePointFilePath_);
    //todo 写入json文件 储存反光板数据到json
    savePosintsWithJsonFile(reflectorPanelPoints_vector_);

    for (const auto& ptr : reflectorPanelPoints_vector_) {
        if(ptr->id==key)
            std::cout<<GREEN << "UPDATE SUCCESS POINT ID: " << ptr->id<< ", X: " << ptr->x << ", Y: " << ptr->y << RESET<<std::endl;
    }


    //todo 3. 删除计算好的所有点位特征-json文件包括缓存
    unorderedEdgeMapNew_.clear();
    unorderedEdgeMapNew_IdKey_.clear();
    clearFile( calculateFilePath_);

    // todo 4. 计算所有点位特征
    // 实现计算所有点位特征并保存为json文件的逻辑
    if(reflectorPanelPoints_vector_.size()==0){
        std::cerr<<RED << "error Saving positions..." <<"reflectorPanelPoints_vector_ is empty" << RESET<<std::endl;
        re.resultTxt = "reflectorPanelPoints_vector_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
    while (iswileTure_){
        if(!iswileTure_){
            return;
        }
        if(!iswirtMatchokReflectoUnordered_map_){
            //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
            matchokReflectorPanelPointEnd_Unordered_map_.clear();
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            break;
        }
    }

    //isMatchok_-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    ReflectorPanelPoint::initInstance();
    // 计算2个点连成的 edge 直线边长数据 包含  saveJSONDataToFile 数据写入jion
    calculateEdge(reflectorPanelPoints_vector_,unorderedEdgeMapNew_,unorderedEdgeMapNew_IdKey_);
    is_trilateration_=true;


    std::cout << "Key " << key << " has been Update from the JSON file.\n";
    re.result_numPoints=reflectorPanelPointAllMaps_.size();
    re.result="ok";
    re.success=true;

}


//-----------------------------------
//todo 部署反光板的同时进行定位 依次储存每一个反光板在 map 下的坐标数据到 json文件中，并计算 点位特征。并创建 ReflectorPanelPoint对象
void handleSavePosintAndCalculatePosintAllFeatureAndStartLocation(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {

    std::cout << "====================================================================== " << std::endl;
    std::cout << "1.Saving positions..." << std::endl;

    if(pointsSaveMap_.size()==0){
        std::cout << "error Saving positions..." <<"pointsSaveMap_ is empty" << std::endl;
        re.resultTxt = "pointsSaveMap_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }

//    if(req.request_shape.size()==0){
//        std::cerr << "error Saving positions..." <<"shape is empty" << std::endl;
//        re.resultTxt = "shape is empty，Please enter CYLINDER OR CUBE " ;
//        re.result="error";
//        re.success=false;
//        return;
//    }

//    if(req.request_shape!="CYLINDER" ||req.request_shape!="CUBE"){
//        std::cerr << "error Saving positions..." <<"shape is invalid" << std::endl;
//        re.resultTxt = "shape is  invalid，Please enter CYLINDER OR CUBE " ;
//        re.result="error";
//        re.success=false;
//        return;
//    }




    //todo 0.0 不要清除反光板的在集合中的缓存数据，因为部署坐标时会多次增加数据，而不是重新开始部署数据，需要重新开始部署数据请先执行 handleDeletePosintAllWithSavePointFilePath 函数。
    //    //清除数据
    //    reflectorPanelPointAllMaps_.clear();
    //    reflectorPanelPoints_vector_.clear();
    //    reflectorPanelPointAllUnordered_maps_.clear();
    //    // 函数用于清空文件中的所有数据
    //    clearFile( savePointFilePath_);

    //todo 1. 初始 单个反光板数据写入缓存集合中 ,反光板的数据 创建 ReflectorPanelPoint 对象并加入 集合 管理，reflectorPanelPointAllMaps_，reflectorPanelPoints_vector_，reflectorPanelPointAllUnordered_maps_
    for (const auto& pair : pointsSaveMap_) {
        Point point=pair.second;
        //const  auto & newPoint = ReflectorPanelPoint::createInstance2
        auto  newPoint = ReflectorPanelPoint::createInstance2(point.id,marker_id_, point.x, point.y, 0.0,0.0,0.0,0.0,1.0);
        //newPoint->shape=req.request_shape;
        newPoint->shape="CYLINDER";
        reflectorPanelPointAllMaps_[newPoint->id]=newPoint;//map集合储存所有点位，
        //reflectorPanelPoints_vector_.insert(newPoint);//列表储存所有点位，通过tf订阅数据，实现自动排序功能
        addInsertNewPointToReflectorPanelPoints_vector(reflectorPanelPoints_vector_,newPoint);
        // 储存所有的点为 std::tuple<double, double> 为 key
        reflectorPanelPointAllUnordered_maps_[std::make_tuple(point.x, point.y)]=newPoint;
        marker_id_=marker_id_+2;
        //打印数据
        std::cout<<"打印数据: "<<newPoint<<std::endl;
    }
    std::cout << "pointsSaveMap_.size(): " <<pointsSaveMap_.size()<< std::endl;
    std::cout << "reflectorPanelPoints_vector_.size(): " <<reflectorPanelPoints_vector_.size()<< std::endl;

    //todo 2. 单个反光板数据写入json文件 储存反光板数据到 json
    savePosintsWithJsonFile(reflectorPanelPoints_vector_);

    //todo 3. 删除计算好的所有点位特征-json文件包括缓存
    unorderedEdgeMapNew_.clear();
    unorderedEdgeMapNew_IdKey_.clear();
    clearFile( calculateFilePath_);

    // todo 4. 计算所有点位特征

    // 实现计算所有点位特征并保存为json文件的逻辑
    if(reflectorPanelPoints_vector_.size()==0){
        std::cerr << "error Saving positions..." <<"reflectorPanelPoints_vector_ is empty" << std::endl;
        re.resultTxt = "reflectorPanelPoints_vector_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
    while (iswileTure_){
        if(!iswileTure_){
            return;
        }
        if(!iswirtMatchokReflectoUnordered_map_){
            //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
            matchokReflectorPanelPointEnd_Unordered_map_.clear();
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            break;
        }
    }

    //isMatchok_ -1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    //todo 这里应该不需要处理，匹配上的数据依旧可以使用
    // ReflectorPanelPoint::initInstance();
//    for (auto& entry : reflectorPanelPointAllMaps_) {
//        if (entry.second && entry.second->isMatchok_ != -1) {
//            entry.second->isMatchok_ = -1;
//        }
//    }


    // 计算2个点连成的 edge 直线边长数据
    calculateEdge(reflectorPanelPoints_vector_,unorderedEdgeMapNew_,unorderedEdgeMapNew_IdKey_);
    is_trilateration_=true;

    re.result_savepointnum=pointsSaveMap_.size();
    re.result="ok";
    re.success=true;
    std::cout << "ok handleSavePosintAndCalculatePosintAllFeatureAndStartLocation " <<pointsSaveMap_.size()<< std::endl;
    pointsSaveMap_.clear();
    std::cout << "====================================================================== " << std::endl;

}

//=========================================================================================================================


//todo-----------------计算所有点位特征--------------------------------
//删除计算好的所有点位特征-json文件包括缓存，匹配上的数据也要清除
void handleDeleteCalculatePosint(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Deleting calculated features for all positions..." << std::endl;
    unorderedEdgeMapNew_.clear();
    unorderedEdgeMapNew_IdKey_.clear();
    clearFile( calculateFilePath_);
    //已经匹配上的点也要清除
    while (iswileTure_){
        if(!iswileTure_){
            return;
        }
        if(!iswirtMatchokReflectoUnordered_map_){
            //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
            matchokReflectorPanelPointEnd_Unordered_map_.clear();
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            break;
        }
    }
    //isMatchok_数据改为-1 ，未开始匹配，//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    ReflectorPanelPoint::initInstance();


    re.result_numPoints=unorderedEdgeMapNew_IdKey_.size();
    re.result="ok";
    re.success=true;
}

//删除已经 MatchokReflectorPanelPointEnd_Unordered_map 匹配上的点
void handleDeleteMatchokReflectorPanelPointEnd_Unordered_map(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Deleting handleDeleteMatchokReflectorPanelPointEnd_Unordered_map..." << std::endl;


    while (iswileTure_){
        if(!iswileTure_){
            return;
        }
        if(!iswirtMatchokReflectoUnordered_map_){
            //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
            matchokReflectorPanelPointEnd_Unordered_map_.clear();
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            break;
        }
    }
    //isMatchok_数据改为-1 ，为开始匹配，//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    ReflectorPanelPoint::initInstance();


    re.result_numPoints=matchokReflectorPanelPointEnd_Unordered_map_.size();
    re.result="ok";
    re.success=true;
}

// 实现重新加载所有点位的json，已经计算好的特征文件的逻辑
void handleReloadJSONDataTxt(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Reloading JSON data from text file..." << std::endl;
    std::unique_lock<std::mutex> lock1(scancallback_mutex_);
    //重现加载意味着反光板特征已经重新计算，所以需要清除已经匹配上的点位。
    while (iswileTure_){
        if(!iswileTure_){
            return;
        }
        if(!iswirtMatchokReflectoUnordered_map_){
            //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
            matchokReflectorPanelPointEnd_Unordered_map_.clear();
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            break;
        }
    }
    //isMatchok_数据改为-1 ，未开始匹配 //-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    ReflectorPanelPoint::initInstance();
//    for (auto& entry : reflectorPanelPointAllMaps_) {
//        if (entry.second && entry.second->isMatchok_ != -1) {
//            entry.second->isMatchok_ = -1;
//        }
//    }
    if(reflectorPanelPointAllMaps_.size()==0){
        std::cerr << "error handleReloadJSONDataTxt positions..." <<"reflectorPanelPointAllMaps_ is empty" << std::endl;
        re.resultTxt = "reflectorPanelPointAllMaps_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }

    //loadCalculateEdgeJSONDataFromFile 中包含了清除缓存数据 unorderedEdgeMapNew.clear();unorderedEdgeMapNew_IdKey.clear();
    loadCalculateEdgeJSONDataFromFile( calculateFilePath_,reflectorPanelPointAllMaps_,unorderedEdgeMapNew_,unorderedEdgeMapNew_IdKey_);

    if(unorderedEdgeMapNew_IdKey_.size()==0){
        std::cerr << "error Reloading JSON data from text file..." <<"unorderedEdgeMapNew_IdKey_.size()==0" << std::endl;
        re.resultTxt = "unorderedEdgeMapNew_IdKey_.size()==0" ;
        re.result_numPoints=unorderedEdgeMapNew_IdKey_.size();
        re.result="error";
        re.success=false;
        return;
    }


    re.result_numPoints=unorderedEdgeMapNew_IdKey_.size();
    re.result="ok";
    re.success=true;
}

//todo 计算所有点位特征
void handleCalculatePosintAllFeature(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {

    // 实现计算所有点位特征并保存为json文件的逻辑
    if(reflectorPanelPoints_vector_.size()==0){
        std::cerr << "error Saving positions..." <<"reflectorPanelPoints_vector_ is empty" << std::endl;
        re.resultTxt = "reflectorPanelPoints_vector_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    unorderedEdgeMapNew_.clear();
    unorderedEdgeMapNew_IdKey_.clear();

    while (iswileTure_){
        if(!iswileTure_){
            return;
        }
        if(!iswirtMatchokReflectoUnordered_map_){
            //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
            matchokReflectorPanelPointEnd_Unordered_map_.clear();
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            break;
        }
    }

    //isMatchok_数据改为-1 ，未开始匹配 //-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
    ReflectorPanelPoint::initInstance();
    std::cout << "bbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbb" << std::endl;

    // 计算2个点连成的 edge 直线边长数据，包含 saveJSONDataToFile
    calculateEdge(reflectorPanelPoints_vector_,unorderedEdgeMapNew_,unorderedEdgeMapNew_IdKey_);
    std::cout << "Saaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa" << std::endl;

    std::cout <<GREEN<< "1.Calculating features for all positions... " << reflectorPanelPoints_vector_.size()<<RESET<< std::endl;
    re.result_numPoints=unorderedEdgeMapNew_IdKey_.size();
    re.result="ok";
    re.success=true;

}
// 实现开启反光板定位的逻辑
void handleStartLocation(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Starting location..." << std::endl;
    // 实现开启反光板定位的逻辑
    if(reflectorPanelPoints_vector_.size()==0||unorderedEdgeMapNew_.size()==0){
        re.resultTxt="请先计算或加载点位特征 unorderedEdgeMapNew_ is empty ";
        std::cerr << re.resultTxt << std::endl;
        re.result="error";
        re.success=false;
        return;
    }
    is_trilateration_=true;
    re.result="ok";
    re.success=true;
}
// 实现停止反光板定位的逻辑
void handleEndLocation(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Ending location..." << std::endl;
    // 实现停止反光板定位的逻辑
    is_trilateration_=false;//是否使用反光板定位
    re.result="ok";
    re.success=true;
}

//============================================================================================
//三边测量计算坐标
void handleTrilateration(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Ending trilateration..." << std::endl;

    re.resultTxt = "function is empty" ;
    re.result="ok";
    re.success=true;
}

//查询数据 用1个反光板坐标id查1个点的数据
void handleFindPosintById(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Ending trilateration..." << std::endl;

    if(req.request_posint_id.size()==0){
        std::cerr << "error findAndPrintData" <<"req.request_posint_id is empty" << std::endl;
        re.resultTxt = "req.request_posint_id is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    std::string  key=req.request_posint_id;
    auto it = reflectorPanelPointAllMaps_.find(key);
    if (it != reflectorPanelPointAllMaps_.end()) {
        std::shared_ptr<ReflectorPanelPoint>   reflectorPanelPoint=it->second;
        std::stringstream resultStream;
        resultStream << "Found key: ";
        resultStream << it->first << " ";
        resultStream <<"                  "<< "[ ";
        // 使用 ptr 指针
        resultStream  <<  reflectorPanelPoint->id<<" ： ";
        resultStream << " ( " << reflectorPanelPoint->x <<" , "<<   reflectorPanelPoint->y <<" ) ";
        resultStream << "] "<< std::endl;
        resultStream << std::endl;
        re.resultTxt = resultStream.str();
        std::cout << GREEN << "data_item： " <<re.resultTxt<<RESET<<std::endl;
        re.result="ok";
        re.success=true;
    } else {
        std::cerr << "Key not found." << std::endl;
        re.resultTxt = "没有查询到数据" ;
        re.result="error";
        re.success=false;
    }
}
//查询数据  用2个反光板坐标id查一条线的前后2个点的数据
void handleFindPosintByIds(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "Ending trilateration..." << std::endl;

    ///**
    std::cout << "----------------------------------调用函数查询数据------------------------------------------" <<std::endl;
    // 调用函数查询数据


    if(req.request_posint_id.size()==0||req.request_posint_id2.size()==0){
        std::cerr << "error findAndPrintData" <<"key1 or key2 is empty" << std::endl;
        re.resultTxt = "req.request_posint_id or req.request_posint_id2 is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    //std::vector<std::string> key = {"ID25", "ID9"};
    std::vector<std::string> keys;
    std::string   key1=req.request_posint_id;
    std::string   key2=req.request_posint_id2;
    keys.push_back(key1);
    keys.push_back(key2);
    double result_distance=0.0;//查询到边长数据
    //返回的具体数据
    std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>>> reslutData;
    bool findAndPrintDataFig  =  findAndPrintData(unorderedEdgeMapNew_IdKey_, keys,reslutData,result_distance);
    if(findAndPrintDataFig){
        double distance = std::get<0>(reslutData);
        std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint>>> data = std::get<1>(reslutData);
        std::cout << GREEN << "查询到边长数据 result_distance： " <<result_distance<<RESET<<std::endl;
        if(data.size()>1){
            std::stringstream resultStream;
            resultStream << "查询到数据： distance " << distance << std::endl;

            for (const auto& data_pair : data) {
                for (const auto &data_item: data_pair) {
                    resultStream << data_item->id << " ( " << data_item->x << " , " << data_item->y << " ) ";
                }
                resultStream << std::endl;
            }
            re.resultTxt = resultStream.str();
            std::cout << GREEN << "data_item： " <<re.resultTxt<<RESET<<std::endl;

        }
        re.result="ok";
        re.success=true;
    }else{
        std::cerr << "没有查询到数据" <<std::endl;
        re.resultTxt = "没有查询到数据" ;
        re.result="error";
        re.success=false;

    }

}
//=============================================================================================================


// 保存雷达强度数据
void handleSaveScanIntensitiesFromJSONDataTxt(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << GREEN<<"Save scanIntensities JSON data from text file..." <<RESET<<  std::endl;
    // 函数用于清空文件中的所有数据
    clearFile( scanIntensitiesFilePath_);
    scan_intensitie_map_.clear();//雷达强度数据要清除
    scan_intensitie_key_.clear();
    //测下来 3.5m 可找到圆心
    //填入雷达强度数据 距离：key 米，value：强度 doule
    scan_intensitie_map_[1]=36025.0;//todo 35957 - 36094  1米以内至少达到 35000 ，以后可以改成数据库存储方式，通过前段录入数据。
    scan_intensitie_map_[2]=35791.5;//  35715 - 35868
    scan_intensitie_map_[3]=35485.0;//  35330 - 35485
    scan_intensitie_map_[4]=35045.5;//  34984 - 35107
    scan_intensitie_map_[5]=34660.5;//  34494 - 34827
    scan_intensitie_map_[6]=33143;//  32994 - 33292
    scan_intensitie_map_[7]=30908.5;//  30505 - 31312
    scan_intensitie_map_[8]= 28576.0;// 28252 - 28900
    scan_intensitie_map_[9]= 28077.5;// 27851 - 28340
    scan_intensitie_map_[10]=27567.0;// 27280 - 27854
    scan_intensitie_map_[11]=27154.0;// 27154

    //储存距离的key
    for (int i = 0; i < scan_intensitie_map_.size(); ++i) {
        scan_intensitie_key_.push_back(i+1);
    }

    //封装雷达对应的强度数的json数据
    std::string jsondata= generateJSONData_with_ScanIntensities( scan_intensitie_map_);
    std::cout <<GREEN<< "Reloading scanIntensities JSON data from text file: " <<jsondata<<RESET<< std::endl;
    //写入txt数据
    saveJSONDataToFile_with_ScanIntensities(jsondata, scanIntensitiesFilePath_);
    re.result_numPoints=scan_intensitie_map_.size();
    re.result="ok";
    re.success=true;
}
// 实现重新加载雷达强度数据
void handleReloadScanIntensitiesFromJSONDataTxt(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {

    std::cout <<GREEN<< "Reloading scanIntensities JSON data from text file..." <<" "<<RESET<< std::endl;
    scan_intensitie_map_.clear();//雷达强度数据要清除
    scan_intensitie_key_.clear();//清除储存距离的key
    //读取json文件 加载文件中的json 雷达对应的强度数据，存入std::map<int,double> &scan_intensitie_map
    loadScanIntensitiesJSONDataFromFile( scanIntensitiesFilePath_,scan_intensitie_map_);
    re.result_numPoints=scan_intensitie_map_.size();
    re.result="ok";
    re.success=true;


}
// delete 雷达强度数据 json
void handleDeleteScanIntensitiesFromJSONDataTxt(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout <<GREEN<< "Delete scanIntensities JSON data from text file... " <<scanIntensitiesFilePath_<<" "<<RESET<< std::endl;
    scan_intensitie_map_.clear();//雷达强度数据要清除
    scan_intensitie_key_.clear();//清除储存距离的key
    clearFile( scanIntensitiesFilePath_);//删除文件中的数据
    re.result_numPoints=scan_intensitie_map_.size();
    re.result="ok";
    re.success=true;
}

//==================================================================================================================================
//显示地图中的所有反光柱，反光板
void handleShowAllPointsMarker(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "handleShowAllPointsMarker..." << std::endl;

    if(reflectorPanelPointAllMaps_.size()==0){
        std::cerr << "error handleShowAllPointsMarker" <<"reflectorPanelPointAllMaps_ is empty" << std::endl;
        re.resultTxt = "reflectorPanelPointAllMaps_ is empty" ;
        re.result="error";
        re.success=false;
        return;
    }
    // 显示所有的反光柱
    fanguangbanShowAllMaker(reflectorPanelPointAllMaps_);
    re.result="ok";
    re.success=true;


}

//显示雷達最大強度值数据,显示正前方的雷达数据
void handleShowScanMaxIntensitiesWithFront(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "handleShowScanMaxIntensities..." << std::endl;

    boost::shared_ptr<sensor_msgs::LaserScan  const>    laserScanmsg = ros::topic::waitForMessage<sensor_msgs::LaserScan>(scan_toptic_name_, ros::Duration(3));

    if (laserScanmsg) {

        if (laserScanmsg->ranges.size() > 0 && laserScanmsg->intensities.size() > 0) {

            //第二个值 正中间 初始化最大值
            //double min_dist_center = std::numeric_limits<double>::max();
            double min_dist_center = 10.0;

            double max_intensity = laserScanmsg->intensities[0];
            size_t max_index = 0;
            double max_range = laserScanmsg->ranges[0];
            double laserTscan_yaw ;//yaw角度
            double laserTscan_x ;
            double laserTscan_y ;

            double laserTscan_yaw_center ;//yaw角度
            double laserTscan_x_center;
            double laserTscan_y_center ;

            tf::Transform scanPoint_to_base_link_transform,center_scanPoint_to_base_link_transform ;
            double x=   -0.1;
            double y=   -0.1;
            for (int i = 0; i < laserScanmsg->ranges.size(); i++) {

                int scan_index = i;                                              //记录高强点数据
                double range = laserScanmsg->ranges[i] ;//+  fangaungzhuRadius_;    //查找最大值确定圆心方法需要加半径，三角函数法不需要
                double intensity = laserScanmsg->intensities[i];
                laserTscan_yaw = laserScanmsg->angle_min + laserScanmsg->angle_increment * scan_index;//yaw角度
                laserTscan_x = range * cos(laserTscan_yaw);
                laserTscan_y = range * sin(laserTscan_yaw);

                std::cout << "111 Max Intensity: " << max_intensity << ", at scan_index: " << scan_index  << ", Range: " << range<< " ,laserTscan_x   : "<<laserTscan_x << " ,laserTscan_y: "<<laserTscan_y<<" ,laser_to_base_link_transform_.getOrigin().getX(): "<<laser_to_base_link_transform_.getOrigin().getX()<<std::endl;
                // 使用姿态 yaw 创建变换 laser
                tf::Transform scanPoint_to_laser_transform;
                scanPoint_to_laser_transform.setOrigin(tf::Vector3(laserTscan_x, laserTscan_y, 0.0));
                tf::Quaternion q;
                q.setRPY(0, 0, laserTscan_yaw);
                scanPoint_to_laser_transform.setRotation(q);

                //if(-0.001<=laserTscan_y&&laserTscan_y<=0.001){
                //=======================================================================================
                /**
               // 筛选正中间 x:正数，y：绝对值最接近于0.0的值） 索引
               if ( laserTscan_y < min_dist_center && laserTscan_x >0 ) {
                   min_dist_center = laserTscan_y;

                   // tf::Transform map_to_odom_transform = base_link_to_map_transform * odom_to_base_link_transform.inverse();
                   scanPoint_to_base_link_transform = laser_to_base_link_transform_ * scanPoint_to_laser_transform;
                   x=   scanPoint_to_base_link_transform.getOrigin().getX();
                   y=   scanPoint_to_base_link_transform.getOrigin().getY();
                   // if (laserScanmsg->intensities[i] > max_intensity) {
                   max_intensity = laserScanmsg->intensities[i];
                   max_index = i;
                   max_range = laserScanmsg->ranges[i];
                   //  }

               }
               */
                //========================================================================================
                // tf::Transform map_to_odom_transform = base_link_to_map_transform * odom_to_base_link_transform.inverse();
                scanPoint_to_base_link_transform = laser_to_base_link_transform_ * scanPoint_to_laser_transform;
                x=   scanPoint_to_base_link_transform.getOrigin().getX();
                y=   scanPoint_to_base_link_transform.getOrigin().getY();

                //  if ( y < min_dist_center && x >0 ) {
                //   if ( laserTscan_y < min_dist_center && laserTscan_x >0) {
                if ( fabs(laserTscan_y) < min_dist_center && laserTscan_x >0 ) {
                    // min_dist_center = y;
                    min_dist_center =fabs(laserTscan_y);
                    center_scanPoint_to_base_link_transform =scanPoint_to_base_link_transform;
                    laserTscan_yaw_center=laserTscan_yaw ;//yaw角度
                    laserTscan_x_center=laserTscan_x;
                    laserTscan_y_center=laserTscan_y ;
                    // if (laserScanmsg->intensities[i] > max_intensity) {
                    max_intensity = laserScanmsg->intensities[i];
                    max_index = i;
                    max_range = laserScanmsg->ranges[i];
                    //  }
                    std::cout << "xxxx Max Intensity: " << max_intensity << ", at Index: " << max_index << ", Range: " << max_range << " ,laserTscan_y: "<<laserTscan_y<<" ,laser_to_base_link_transform_.getOrigin().getX(): "<<laser_to_base_link_transform_.getOrigin().getX()<<std::endl;

                }



//                    if(-0.001<=y && y<=0.001){
//                    break;
//                }

            }

            x=   center_scanPoint_to_base_link_transform.getOrigin().getX();
            y=   center_scanPoint_to_base_link_transform.getOrigin().getY();

            double d=  std::sqrt(std::pow(x, 2) + std::pow(y, 2));
            std::cout << "強度值 Intensity: " << max_intensity << ", 正中间雷达索引 Index: " << max_index << ",正中间雷达距离 Range: " << max_range << std::endl;
            //  re.resultTxt = "強度值 Intensity: " + std::to_string(max_intensity) + ", 正中间雷达索引 Index: " + std::to_string(max_index) + ", 正中间雷达距离Range: " + std::to_string(max_range)+ ", laserTscan_x: " + std::to_string(laserTscan_x)+ ", laserTscan_y: " + std::to_string(laserTscan_y)+ ", scanPoint_to_base_link_x: " + std::to_string(scanPoint_to_base_link_transform.getOrigin().getX())+ ", scanPoint_to_base_link_y: " + std::to_string(scanPoint_to_base_link_transform.getOrigin().getY())+ ", scanPoint_to_base_link_d: " + std::to_string(d);
            re.resultTxt = " Intensity: " + std::to_string(max_intensity) + ",  Index: " + std::to_string(max_index) + ", Range: " + std::to_string(max_range)+ ", laserTscan_x: " + std::to_string(laserTscan_x_center)+ ", laserTscan_y: " + std::to_string(laserTscan_y_center)+ ", scanPoint_to_base_link_x: " + std::to_string(x)+ ", scanPoint_to_base_link_y: " + std::to_string(y)+ ", scanPoint_to_base_link_d: " + std::to_string(d);

            re.resultMaxIntensityRangeIndex =max_index;  //最大強度值对应的索引
            re.resultMaxIntensityRange = max_range; //最大強度值对应的距离
            re.resultMaxIntensity = max_intensity;//显示雷達最大強度值数据
            re.result = "ok";
            re.success = true;
        }else {
            // 如果强度数据或范围数据为空，可以添加相应的处理逻辑
            std::cerr << "No intensity data or range data available." << std::endl;
            re.result="No intensity data or range data available";
            re.success=false;
        }


    } else {
        // 如果在指定时间内未接收到消息，也可以添加处理逻辑
        std::cerr << "No message received within the specified time." << std::endl;
        re.resultTxt = "No message received within the specified time." ;
        re.result="error";
        re.success=false;
    }

}

void handleShowScanMaxIntensitiesWithFrontNew(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "handleShowScanMaxIntensities..." << std::endl;

    boost::shared_ptr<sensor_msgs::LaserScan  const>    laserScanmsg = ros::topic::waitForMessage<sensor_msgs::LaserScan>(scan_toptic_name_, ros::Duration(3));

    if (laserScanmsg) {

        if (laserScanmsg->ranges.size() > 0 && laserScanmsg->intensities.size() > 0) {

            size_t forward_index = (laserScanmsg->angle_max - 0.0) / laserScanmsg->angle_increment; // 查找正前方的索引
            double forward_distance = laserScanmsg->ranges[forward_index]; // 小车正前方的距离
            std::cout << "小车正前方雷达到达障碍物的距离 Forward Distance: " << forward_distance << "小车正前方雷达索引 forward_index: " << forward_distance << std::endl;
            // 其他代码逻辑，例如寻找最大强度点
            double max_intensity = laserScanmsg->intensities[0];
            size_t max_index = 0;
            double max_range = laserScanmsg->ranges[0];
            double laserTscan_yaw ;//yaw角度
            double laserTscan_x ;
            double laserTscan_y ;
            tf::Transform scanPoint_to_base_link_transform ;
            double x=   -0.1;
            double y=   -0.1;
            for (int i = 0; i < laserScanmsg->ranges.size(); i++) {
                int scan_index = i;                                              //记录高强点数据
                double range = laserScanmsg->ranges[i] ;//+  fangaungzhuRadius_;    //查找最大值确定圆心方法需要加半径，三角函数法不需要
                double intensity = laserScanmsg->intensities[i];
                laserTscan_yaw = laserScanmsg->angle_min + laserScanmsg->angle_increment * scan_index;//yaw角度
                laserTscan_x = range * cos(laserTscan_yaw);
                laserTscan_y = range * sin(laserTscan_yaw);

                std::cout << "Max Intensity: " << max_intensity << ", at Index: " << max_index << ", Range: " << max_range << " ,laserTscan_y: "<<laserTscan_y<<" ,laser_to_base_link_transform_.getOrigin().getX(): "<<laser_to_base_link_transform_.getOrigin().getX()<<std::endl;
                // 使用姿态 yaw 创建变换 laser
                tf::Transform scanPoint_to_laser_transform;
                scanPoint_to_laser_transform.setOrigin(tf::Vector3(laserTscan_x, laserTscan_y, 0.0));
                tf::Quaternion q;
                q.setRPY(0, 0, laserTscan_yaw);
                scanPoint_to_laser_transform.setRotation(q);
                // tf::Transform map_to_odom_transform = base_link_to_map_transform * odom_to_base_link_transform.inverse();
                scanPoint_to_base_link_transform = laser_to_base_link_transform_ * scanPoint_to_laser_transform;
                x=   scanPoint_to_base_link_transform.getOrigin().getX();
                y=   scanPoint_to_base_link_transform.getOrigin().getY();

                if (intensity > max_intensity) {
                    max_intensity = intensity;
                    max_index = i;
                    max_range = laserScanmsg->ranges[i];
                }
                //小车正前方
//                    if(-0.001<=y && y<=0.001){
//                        break;
//                    }
            }//for (int i = 0; i < laserScanmsg->ranges.size(); i++)

            std::cout << "最大强度值 Max Intensity: " << max_intensity << ", at Index: " << max_index << ", Range: " << max_range << std::endl;


            double d=  std::sqrt(std::pow(x, 2) + std::pow(y, 2));
            std::cout << "Max Intensity: " << max_intensity << ", at Index: " << max_index << ", Range: " << max_range << std::endl;
            re.resultTxt = "Max Intensity: " + std::to_string(max_intensity) + ", at Index: " + std::to_string(max_index) + ", Range: " + std::to_string(max_range)+ ", laserTscan_x: " + std::to_string(laserTscan_x)+ ", laserTscan_y: " + std::to_string(laserTscan_y)+ ", scanPoint_to_base_link_x: " + std::to_string(scanPoint_to_base_link_transform.getOrigin().getX())+ ", scanPoint_to_base_link_y: " + std::to_string(scanPoint_to_base_link_transform.getOrigin().getY())+ ", scanPoint_to_base_link_d: " + std::to_string(d);
            re.resultMaxIntensityRangeIndex =max_index;  //最大強度值对应的索引
            re.resultMaxIntensityRange = max_range; //最大強度值对应的距离
            re.resultMaxIntensity = max_intensity;//显示雷達最大強度值数据
            re.result = "ok";
            re.success = true;
        }else {
            // 如果强度数据或范围数据为空，可以添加相应的处理逻辑
            std::cerr << "No intensity data or range data available." << std::endl;
            re.result="No intensity data or range data available";
            re.success=false;
        }



    } else {
        // 如果在指定时间内未接收到消息，也可以添加处理逻辑
        std::cerr << "No message received within the specified time." << std::endl;
        re.resultTxt = "No message received within the specified time." ;
        re.result="error";
        re.success=false;
    }

}


//显示雷達最大強度值数据
void handleShowScanMaxIntensities(const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& re) {
    std::cout << "handleShowScanMaxIntensities..." << std::endl;

    boost::shared_ptr<sensor_msgs::LaserScan  const>    laserScanmsg = ros::topic::waitForMessage<sensor_msgs::LaserScan>(scan_toptic_name_, ros::Duration(3));

    if (laserScanmsg) {
        //
        if (laserScanmsg->ranges.size() > 0 && laserScanmsg->intensities.size() > 0) {
            double max_intensity = laserScanmsg->intensities[0];
            size_t max_index = 0;
            double max_range = laserScanmsg->ranges[0];

            for (size_t i = 1; i < laserScanmsg->intensities.size(); ++i) {
                if (laserScanmsg->intensities[i] > max_intensity) {
                    max_intensity = laserScanmsg->intensities[i];
                    max_index = i;
                    max_range = laserScanmsg->ranges[i];
                }
            }

            std::cout << "Max Intensity: " << max_intensity << ", at Index: " << max_index << ", Range: " << max_range << std::endl;
            re.resultTxt = "Max Intensity: " + std::to_string(max_intensity) + ", at Index: " + std::to_string(max_index) + ", Range: " + std::to_string(max_range);
            re.resultMaxIntensityRangeIndex =max_index;  //最大強度值对应的索引
            re.resultMaxIntensityRange = max_range; //最大強度值对应的距离
            re.resultMaxIntensity = max_intensity;//显示雷達最大強度值数据
            re.result = "ok";
            re.success = true;
        }else {
            // 如果强度数据或范围数据为空，可以添加相应的处理逻辑
            std::cerr << "No intensity data or range data available." << std::endl;
            re.result="No intensity data or range data available";
            re.success=false;
        }




//        if (laserScanmsg->ranges.size() > 0 && laserScanmsg->intensities.size() > 0) {
//            auto max_intensity_it = std::max_element(laserScanmsg->intensities.begin(), laserScanmsg->intensities.end());
//            //用来确定最大强度值在intensities向量中的索引位置。
//            size_t max_index = std::distance(laserScanmsg->intensities.begin(), max_intensity_it);
//
//            double max_intensity = *max_intensity_it;
//            double max_range = laserScanmsg->ranges[max_index]; // 获取最大强度对应的距离数据
//
//            std::cout << "Max Intensity: " << max_intensity << ", at Index: " << max_index << ", Range: " << max_range << std::endl;
//            re.resultTxt = "Max Intensity: " + std::to_string(max_intensity) + ", at Index: " + std::to_string(max_index)+", Range: " +std::to_string(max_range);
//
////            re.resultMaxIntensityRangeIndex = std::to_string(max_range);
////            re.resultMaxIntensityRange = std::to_string(max_range);
////            re.resultMaxIntensity = std::to_string(max_intensity);
//            re.result="ok";
//            re.success=true;
//        } else {
//            // 如果强度数据或范围数据为空，可以添加相应的处理逻辑
//            std::cerr << "No intensity data or range data available." << std::endl;
//            re.result="No intensity data or range data available";
//            re.success=false;
//        }
    } else {
        // 如果在指定时间内未接收到消息，也可以添加处理逻辑
        std::cerr << "No message received within the specified time." << std::endl;
        re.resultTxt = "No message received within the specified time." ;
        re.result="error";
        re.success=false;
    }

}



//======================================================================================================
//创建一个函数，用于填充 request_handlers 映射表
std::map<std::string, HandlerFunction> createRequestHandlers() {
    std::map<std::string, HandlerFunction> handlers;
    //用于自动生成键值对
    ADD_HANDLER(handlers, handleSavePosint);//todo 依次储存每一个反光板在 map 下的坐标数据到 json文件中，并创建ReflectorPanelPoint对象
    ADD_HANDLER(handlers, handleDeletePosint);// 实现删除单个鼠标获取rviz的缓存点 pointsSaveMap_ 位的逻辑
    // 添加其他处理函数
    ADD_HANDLER(handlers, handleDeletePosintAll);// 实现删除所有缓存点位 pointsSaveMap_ 的逻辑，部署时鼠标获取的点。
    ADD_HANDLER(handlers, handleDeletePosintAllWithSavePointFilePath);// 实现删除所有json文件中的 所有点位包括 缓存 pointAllMaps_
    ADD_HANDLER(handlers, handleDeletePosintWithJsonFile);// 实现删除单个json文件中的点位，删除单个数据后会全部重新计算特征数据
    ADD_HANDLER(handlers, handleUpdatePosintWithJsonFile);//更新单个json中的数据
    ADD_HANDLER(handlers, handleSavePosintAndCalculatePosintAllFeatureAndStartLocation);//部署反光板的同时进行定位 依次储存每一个反光板在 map 下的坐标数据到 json文件中，并创建 ReflectorPanelPoint对象
    ADD_HANDLER(handlers, handleDeleteMatchokReflectorPanelPointEnd_Unordered_map); //删除已经 MatchokReflectorPanelPointEnd_Unordered_map 匹配上的点
    ADD_HANDLER(handlers, handleReloadJSONPosintAll);// 重新获取的JSON中的反光板单个点位
    ADD_HANDLER(handlers, handleCalculatePosintAllFeature);//todo 计算所有点位特征
    ADD_HANDLER(handlers, handleDeleteCalculatePosint);//删除计算好的所有点位特征-json文件包括缓存，匹配上的数据也要清除
    ADD_HANDLER(handlers, handleReloadJSONDataTxt);// 实现重新加载所有点位的json，已经计算好的特征文件的逻辑
    ADD_HANDLER(handlers, handleStartLocation);// 实现开启反光板定位的逻辑
    ADD_HANDLER(handlers, handleEndLocation);// 实现停止反光板定位的逻辑
    ADD_HANDLER(handlers, handleFindPosintById);//查询数据 用1个反光板坐标id查1个点的数据
    ADD_HANDLER(handlers, handleFindPosintByIds);//查询数据  用2个反光板坐标id查一条线的前后2个点的数据
    ADD_HANDLER(handlers, handleTrilateration);//三边测量计算坐标
    ADD_HANDLER(handlers, handleSaveScanIntensitiesFromJSONDataTxt);//保存雷达强度数据
    ADD_HANDLER(handlers, handleReloadScanIntensitiesFromJSONDataTxt);//实现重新加载雷达强度数据
    ADD_HANDLER(handlers, handleDeleteScanIntensitiesFromJSONDataTxt);// delete 雷达强度数据 json
    ADD_HANDLER(handlers, handlePrintReflectorPanelPoints_vector);////控制台打印所有已经转为ReflectorPanel反光板，并且排序好的反光板数据 std::multiset<std::shared_ptr<ReflectorPanelPoint>, CompareByDistanceAndX> reflectorPanelPoints_vector_;
    ADD_HANDLER(handlers, handlePrintReflectorPanelPointAllUnordered_maps);//控制台打印所有已经转为ReflectorPanel反光板，std::tuple<double, double> 为 key,map下的反光板全局坐标 std::tuple<x, y> 为key， std::unordered_map<std::tuple<double, double>, std::shared_ptr<ReflectorPanelPoint>, TupleHash, TupleEqual>  reflectorPanelPointAllUnordered_maps_;
    ADD_HANDLER(handlers, handlePrintMatchokReflectorPanelPointEnd_Unordered_map);//打印 储存匹配上的点位，map下的反光板全局坐标 std::tuple<x, y> 为key std::unordered_map<std::tuple<double, double>, std::shared_ptr<ReflectorPanelPoint>, TupleHash, TupleEqual>  matchokReflectorPanelPointEnd_Unordered_map_;
    ADD_HANDLER(handlers, handlePrintSavePosint);//控制台打印已经用鼠标点击地图选择好的反光板
    ADD_HANDLER(handlers, handlePrintPosintAllMap);//控制台打印所有已经转为ReflectorPanel反光板std::shared_ptr<ReflectorPanelPoint>对象的反光板数据
    ADD_HANDLER(handlers, handlePrintPrintIndices);
    ADD_HANDLER(handlers, handlePrintUnorderedEdgeMapNew_IdKey);////打印 计算好2点位带有边长数据的函数 std::unordered_map<std::vector<std::string>, std::tuple<double, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>,VectorStringHash, VectorStringEqual> unorderedEdgeMapNew_IdKey_;
    ADD_HANDLER(handlers, handlePrintUnorderedEdgeMapNew);//打印 计算好2点位带有边长数据的函数 std::unordered_map<std::string , std::vector<std::tuple<std::vector<double>, std::vector<std::vector<std::shared_ptr<ReflectorPanelPoint> >>>>> unorderedEdgeMapNew_;
    ADD_HANDLER(handlers, handleShowAllPointsMarker);//显示地图中的所有反光柱，反光板
    ADD_HANDLER(handlers, handleShowScanMaxIntensities);//显示雷達最大強度值数据
    ADD_HANDLER(handlers, handleShowScanMaxIntensitiesWithFront);//显示雷達正前方的雷达强度值
    ADD_HANDLER(handlers, handleSaveMapTbaselinkPose);

    return handlers;
}


//----------------------------------------------------------------------------------------------------------------------


// rviz地图取反光板点位，/clicked_point 回调函数，处理接收到的消息
void clickedPointCallback(const geometry_msgs::PointStamped::ConstPtr& msg)
{


    Point point;
    //  point.id=generate_shortened_string();//唯一id
    point.id = "ID" + std::to_string(idCounter_++);
    point.shape="CYLINDER";
    point.range=scan_size_;
    point.intensity=intensities_size_;

    point.x = msg->point.x;
    point.y = msg->point.y;
    point.z = msg->point.z;
    //当point.rw被设置为1，而point.rx, point.ry, point.rz都是0时，这个四元数表示的是一个没有旋转的状态，即刚体保持其初始方向不变。
    point.rx = 0.0;
    point.ry = 0.0;
    point.rz = 0.0;
    point.rw=  1.0;
    pointsSaveMap_[point.id]=point;

    // 打印接收到的消息
    ROS_INFO("Received clicked point: x = %f, y = %f, z = %f", msg->point.x, msg->point.y, msg->point.z);
//    re.result="ok";
//    re.success=true;
}



//重定位
void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg)
{

    try{


        if(ispublishInitialPosefig_){
            ROS_WARN("不能接收自己发布的重定位消息");
            return;
        }

        std::unique_lock<std::mutex> lock(scancallback_mutex_);
        std::unique_lock<std::mutex> lock1(queue_mutex_);
        //   static    tf::TransformListener tf_listener_odom;

        // boost::recursive_mutex::scoped_lock prl(configuration_mutex_);
        if(msg.header.frame_id == "")
        {
            // This should be removed at some point
            ROS_WARN("Received initial pose with empty frame_id.  You should always supply a frame_id.");
            return;
        }

        tf::StampedTransform base_link_to_odom_transform;
        try {
            // msg.header.stamp ros::Time(0)
            tf_listener_new_->waitForTransform(odom_frame_id_, base_link_frame_id_,
                                               msg.header.stamp, ros::Duration(1.0));
            tf_listener_new_->lookupTransform(odom_frame_id_, base_link_frame_id_, msg.header.stamp, base_link_to_odom_transform);
        } catch (tf::TransformException& ex) {

            ROS_WARN("%s", ex.what());
            return;
        }
        // 将 base_link 在 map 下的坐标转换到 odom 坐标系下
        tf::Transform base_link_to_map;
        tf::poseMsgToTF(msg.pose.pose, base_link_to_map);

        // mapTodom  计算 map 到 odom 的变换   mapTbase_link * odomTbase_link.inverse()
        tf::Transform odom_to_map = base_link_to_map * base_link_to_odom_transform.inverse();
        tf_last_odom_to_map_transform_=odom_to_map;
        while (iswileTure_){
            if(!iswileTure_){
                return;
            }
            if(!iswirtMatchokReflectoUnordered_map_){
                //因为时时重新计算特征数据，所以 已经匹配上的点也要清除
                matchokReflectorPanelPointEnd_Unordered_map_.clear();
                std::this_thread::sleep_for(std::chrono::milliseconds(2));
                break;
            }// if(!iswirtMatchokReflectoUnordered_map_)
        }//  while (iswileTure_){

        //isMatchok_数据改为-1 ，为开始匹配 //-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
        ReflectorPanelPoint::initInstance();

//    // 如果需要，可以将 map_to_odom 转换为 geometry_msgs::Pose 并发布
//    geometry_msgs::PoseStamped odom_pose_msg;
//    odom_pose_msg.header.frame_id = odom_frame_id_;
//    odom_pose_msg.header.stamp = msg.header.stamp;
//    tf::poseTFToMsg(odom_to_map, odom_pose_msg.pose);
//    // 此时，odom_pose_msg 包含了 odom 在 map 坐标系下的坐标

    }catch (tf::TransformException& ex) {
        ROS_WARN("%s", ex.what());
    }



}
//重定位
void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
    handleInitialPoseMessage(*msg);
}


// 回调函数，当参数发生变化时被调用
void dynamic_reconfigure_callback(fanguangbantrilateration::TrilaterationParamConfig &config, uint32_t level)
{

    std::lock_guard<std::mutex> lock(m_node_mutex_);
    if(!setup_)
    {

        config = param_config_;
        default_config_ = param_config_;
        last_config_ = config;
        setup_ = true;
        return;
    }
    //  ROS_WARN_STREAM("start 参数服务器： set dynamic_reconfigure" );


    if(config.restore_defaults) { //重置
        config = default_config_;
        //if someone sets restore defaults on the parameter server, prevent looping
        config.restore_defaults = false;
    }
    std::cout<<MAGENTA << "更新 point 数据 isupdate_point = true  " <<" ------------  " <<" 查询 point 数据 isupdate_point = false  "  << RESET<<std::endl;

    try {

        map_frame_id_=config.map_frame_id;
        base_link_frame_id_=config.base_frame_id;
        odom_frame_id_=config.odom_frame_id;
        laser_frame_id_=config.laser_frame_id;
        imu_frame_id_=config.imu_frame_id;

        end_scan_index_=config.end_scan_index;

        fangaungbanHeight_=config.fangaungbanHeight;
        fangaungbanWidth_=config.fangaungbanWidth;
        fangaungbanLength_=config.fangaungbanLength;

        fangaungzhuRadius_=config.fangaungzhuRadius;//与图形无关
        fangaungzhuHeight_=config.fangaungzhuHeight;
        fangaungzhuWidth_=config.fangaungzhuWidth;
        fangaungzhuLength_=config.fangaungzhuLength;


        //如果平均曲率超过阈值，认为是反光柱
        curvature_threshold_=config.curvature_threshold;
        //在阀值距离范围内，判断 curvature_threshold_ 是反光柱还是反光板
        curvature_range_min_threshold_=config.curvature_range_min_threshold;//距离小于阀值有效
        curvature_range_max_threshold_=config.curvature_range_max_threshold;//距离大于阀值有效


        //匹配上的反光板颜色
        a_=config.a;//透明度
        r_=config.r;
        g_=config.g;
        b_=config.b;


        scan_intensitie_=config.scan_intensitie;
        intensitie_threshole_=config.intensitie_threshole;
        range_threshole_=config.range_threshole;
        matchok_timeout_=config.matchok_timeout;
        edgesthreshold_=config.edgesthreshold;
        anglesthreshold_=config.anglesthreshold;

        isCircular_threshold_=config.isCircular_threshold;
        isLine_threshold_=config.isLine_threshold;

        time_log_fig_=config.time_log_fig;
        test_num_=config.test_num;//参数服务测试使用，没有实际意思。通过这个参数可以动态的调试代码，把加入到代码调试位置
        //点的阀值，点云聚类，分割出各个反光柱。至少连续三个点强度超过   强度阀值   即默认为反光柱,单独点滤除
        points_size_threshold_=config.points_size_threshold;
        findPolygonIndices_threshold_=config.findPolygonIndices_threshold;

        //【过滤掉反光柱两俩之间距离太近（在 filter_tow_ont_distance_threshold_ 阀值内就剔除）的点（有可能是数据波动产生的）】
        filter_tow_ont_distance_threshold_=config.filter_tow_ont_distance_threshold;

        isSendTf_=config.isSendTf;
        isFirtFanguangbanShowAllMaker_=config.isFirtFanguangbanShowAllMaker;
        is_trilateration_=config.is_trilateration;
        isSend_publish_circle_pointCloud_=config.isSend_publish_circle_pointCloud;
        isStartAreAnglesEqualNew_=config.isStartAreAnglesEqualNew;
        isShowFanGuanBanPoint_=config.isShowFanGuanBanPoint;
        isShowMatchokReflectorFanGuanBanPoint_=config.isShowMatchokReflectorFanGuanBanPoint;
        isReadMatchokReflectorPanel_=config.isReadMatchokReflectorPanel;//  true：当有匹配上反光板后，优先使用匹配好的反光板数据。false：不使用匹配好的反光板数据，一直只使用反光板特征匹配算法
        isShowFanGuanBanmarkerPath_=config.isShowFanGuanBanmarkerPath;//是否显示反光板与小车之间的匹配连线

        epsilon_=config.epsilon;//todo  重要数据 误差精度影响 map 的key搜索，误差不能大于两个反光板直接的距离
        time_milliseconds_=config.time_milliseconds;//匹配上的点的更新速度 ms,点为marker的发布频率
        scan_distances_threshole_=config.scan_distances_threshole;//超过这个值为无效数据，雷达有效范围，也在预测范围内，单位m

        mapTbase_link_pos_threshold_=config.mapTbase_link_pos_threshold;//3边定位最小二乘 计算得到的 mapTbase_link  位置阈值
        mapTbase_link_rot_threshold_=config.mapTbase_link_rot_threshold;//3边定位最小二乘 计算得到的  角度阈值，例如10度

        calculateEdge_threshold_=config.calculateEdge_threshold;//calculateEdge_threshold_ 计算特征时的距离阀值，计算距离阀值，两点过远就不需要计算了



        targetTrilaterationTimeOut_=config.targetTrilaterationTimeOut;///三边定位最小二乘法计算得到的mapTbase_link结果不符合阀值条件的设定的目标超时时间


        //超时说明没有订阅到coatmap的数据
        matchcostmapwithscanTimeOut_=config.matchcostmapwithscanTimeOut;


        // 需要分优先顺序，环境中反光板数据上千，上万个选择 isFindReflectorPanelPoints_vector_first_： true，反光板数据上百个isFindReflectorPanelPoints_vector_first_：值随意，最佳选择 isFindReflectorPanelPoints_vector_first_： false。
        isFindReflectorPanelPoints_vector_first_=config.isFindReflectorPanelPoints_vector_first;
        ////当通过key在map中查不到数据时是否遍历列表集合来查询数据,配合 reflectorPanelPoints_vector_size_ 使用代表需要遍历前面多少数据，reflectorPanelPoints_vector_集合中的数据是安预测可能性排序的，可能性越大排序越靠前
        isFindReflectorPanelPoints_vector_=config.isFindReflectorPanelPoints_vector;
        ////是匹配时否搜索 reflectorPanelPointAllUnordered_maps中 的所有数据
        isFindRlectorPanelPointAllUnordered_maps_=config.isFindRlectorPanelPointAllUnordered_maps;
        ////具体评估雷达可见范围内可预测多好数据，reflectorPanelPoints_vector 集合中的数据是安预测可能性排序的，可能性越大排序越靠前
        reflectorPanelPoints_vector_size_=config.reflectorPanelPoints_vector_size;// 1/8
        //isUsingThirdPartyLocalization_ 是否配合第三方定位算法一起使用，比如 amcl等。true：表示一起使yong
        isUsingThirdPartyLocalization_=config.isUsingThirdPartyLocalization;
        if(config.firstTrilateration_with_yawFig&&firstTrilateration_with_yawFig_== false){
            //记录是否已经通过3边定位成功了一次，第一次定位成功就设置true
            firstTrilateration_with_yawFig_=config.firstTrilateration_with_yawFig;
            config.firstTrilateration_with_yawFig=false;
        }else if(config.firstTrilateration_with_yawFig && firstTrilateration_with_yawFig_== true){
            firstTrilateration_with_yawFig_=false;
            config.firstTrilateration_with_yawFig=false;
        }

        //角度方差 定义一个方差阀值,判断当前通过3边定位算法出来的结果与里程计的方差是否小于阀值
        variance_yaw_threshold_=config.variance_yaw_threshold;
        //满足角度调整阀值，可以设置 variance_yaw_threshold_ 的倍数作为参数
        variance_adjustment_yaw_threshold_=config.variance_adjustment_yaw_threshold;


        //位置方差 定义一个方差阀值,判断当前通过3边定位算法出来的结果与里程计的方差是否小于阀值
        variance_pose_threshold_=config.variance_pose_threshold;
        //满足位置调整阀值，可以设置 variance_pose_threshold_ 的倍数作为参数
        variance_adjustment_pose_threshold_=config.variance_adjustment_pose_threshold;


        //位置方差 超过这个精度就使用反光柱定位数据进行定位纠正
        variance_min_pose_lerance_=config.variance_min_pose_lerance;
        //角度方差 超过这个精度就使用反光柱定位数据进行定位纠正
        variance_min_yaw_lerance_=config.variance_min_yaw_lerance;


        //匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿,如果score 大于阀值 1.2且小于阀值  等于3.0，表示匹配成功的。ComputeCurrentPose 10090
        score_min_threshold_=config.score_min_threshold;
        //匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿,如果score 大于阀值 1.2且小于阀值  等于3.0，表示匹配成功的 ComputeCurrentPose 10090
        score_max_threshold_=config.score_max_threshold;
       //true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。false 可以尝试在宽阔且雷达打不到墙的环境中使用，需要满足一部分雷达点能打到墙壁。
        is_use_scan_points_map_size_=config.is_use_scan_points_map_size;


        //连续出现3次定位失败就重新置为 firstTrilateration_with_yawFig_ = fasle。重新定位
        isNotMin_varianceNum_target_=config.isNotMin_varianceNum_target;

        //是否启动判断是否是原地旋转功能，启动后 原地旋转 不会更新定位数据
        is_start_place_rotation_=config.is_start_place_rotation;
        //is_start_place_rotation=true 后会加载 rotate_in_place_variance_multiplier 在原地旋转时放大定位精度，从而减少定位的抖动带来的移动机器人反复调整位置.
        rotate_in_place_variance_multiplier_=config.rotate_in_place_variance_multiplier;
        //----------------------------- matchCostmapWithScan.cpp --------------------------
        //match_costmap_with_scan_service 验证移动机器人位置以及雷达点云匹配律是否可靠 matchCostmapWithScan.cpp
        isPublish_matchcostmapwithscan_mssage_=config.isPublish_matchcostmapwithscan_mssage;
        free_cell_=config.free_cell;
        occupied_cell_=config.occupied_cell;
        unknown_cell_=config.unknown_cell;
        matchingcostma_radius_=config.matchingcostma_radius;
        matchingcostma_threshold_=config.matchingcostma_threshold;
        //-------------------------------------------------设置另一类 matchCostmapWithScan.cpp 中的参数--------------------------------------------------------------------------
        // 创建一个服务消息对象,判断移动机器人计算出来的坐标数据是否在自由移动区域
        fanguangbantrilateration::matchcostmapwithscan_srvs matchcostmapwithscan_srv;
        matchcostmapwithscan_srv.request.request_type = "handleSetParam";  // 设置请求类型
        matchcostmapwithscan_srv.request.request_isPublish_matchcostmapwithscan_mssage= isPublish_matchcostmapwithscan_mssage_;
        matchcostmapwithscan_srv.request.request_free_cell= free_cell_;
        matchcostmapwithscan_srv.request.request_occupied_cell= occupied_cell_;
        matchcostmapwithscan_srv.request.request_unknown_cell= unknown_cell_;
        matchcostmapwithscan_srv.request.request_radius= matchingcostma_radius_;
        matchcostmapwithscan_srv.request.request_threshold= matchingcostma_threshold_;
        matchcostmapwithscan_srv.request.request_is_use_scan_points_map_size= is_use_scan_points_map_size_;

        // 发送服务请求
        if (match_costmap_with_scan_service_client_.call(matchcostmapwithscan_srv))
        {
            if( matchcostmapwithscan_srv.response.success){
                std::cout <<"设置另一类 matchCostmapWithScan.cpp 验证移动机器人位置以及雷达点云匹配律是否可靠 中的参 ok ！"<< std::endl;
            }

        }

        //---------------------------------------------------------------------------



        bool isupdate_point = config.isupdate_point;//更新数据
        bool isslectPoint = config.isslectPoint;//isslectPoint=false: 查询数据
        double update_factor = config.update_factor;//更新的系数
        bool update_up_x_button = config.update_up_x_button;
        bool update_down_x_button = config.update_down_x_button;
        bool update_up_y_button = config.update_up_y_button;
        bool update_down_y_button = config.update_down_y_button;
        std::string update_point_id = config.update_point_id;

        if(is_trilateration_){
            queue_cond_var_.notify_one();//唤醒线程
        }



        //查询数据  isupdate_point=false 时为查询数据给参数服务器 , isupdate_point=false: 查询数据  isupdate_point=true:  更新数据
        if(isslectPoint&&config.update_point_id!="EMPTY"){
            std::cout<<BLUE  <<" 查询 point 数据 "  << RESET<<std::endl;

            //查找类ReflectorPanelPoint对象中的数据
            std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint;
            bool  getInstancefig = ReflectorPanelPoint::getInstance(update_point_id,reflectorPanelPoint);
            if(!getInstancefig){
                config.isslectPoint=false;
                std::cout <<RED<< "error dynamic_reconfigure_callback select a position..." <<"reflectorPanelPoint is empty key： " << update_point_id<<" "<<RESET<<std::endl;
                return;
            }
            config.update_point_x=reflectorPanelPoint->x;//查询数据
            config.update_point_y=reflectorPanelPoint->y;//查询数据
            std::cout<<GREEN << "SELECT SUCCESS POINT ID: " <<reflectorPanelPoint->id << ", X: " << reflectorPanelPoint->x << ", Y: " << reflectorPanelPoint->y << RESET<<std::endl;

            // 显示一个的反光柱,只显示一次
            fanguangbanShowOneMaker( reflectorPanelPoint);

            config.isslectPoint=false;
        }//  查询数据 if(!isupdate_point&&config.update_point_id!="EMPTY")
        //-----------------------------------------------------------------------------------------
        //x  up
        if(update_up_x_button&&config.update_point_id!="EMPTY"){

            config.update_point_x=config.update_point_x+update_factor;
            double update_point_x= config.update_point_x;
            std::cout<<BLUE << "更新 point 数据 update_up_x_button  " << RESET<<std::endl;

            //查找类ReflectorPanelPoint对象中的数据
            std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint;
            bool  getInstancefig = ReflectorPanelPoint::getInstance(update_point_id,reflectorPanelPoint);
            if(!getInstancefig){
                std::cout <<RED<< "error dynamic_reconfigure_callback Update a position..." <<"reflectorPanelPoint is empty key： " << update_point_id<<" "<<RESET<<std::endl;
                return;
            }
//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
            reflectorPanelPoint->isMatchok_=-1;
            reflectorPanelPoint->currentIsMatchok_=-1;
            reflectorPanelPoint->isAssignment_=-1;//1:不可以赋值 ，-1：可以赋值
            reflectorPanelPoint->x=update_point_x;//更新数据应为是指针数据，所以reflectorPanelPointAllMaps_数据也会跟着改
            reflectorPanelPoint->isAssignment_=1;

            //更改集合中的数据
            reflectorPanelPointAllMaps_[reflectorPanelPoint->id]=reflectorPanelPoint;

//            pointsSaveMap_.clear();
//            // 函数用于清空文件中的所有数据
//            clearFile( savePointFilePath_);
//            //todo 写入json文件 储存反光板数据到json
//            savePosintsWithJsonFile(reflectorPanelPoints_vector_);

            // 显示一个的反光柱,只显示一次
            fanguangbanShowOneMaker( reflectorPanelPoint);

            for (const auto& ptr : reflectorPanelPoints_vector_) {
                if(ptr->id==update_point_id)
                    std::cout<<GREEN << "UPDATE SUCCESS POINT ID: " << ptr->id<< ", X: " << ptr->x << ", Y: " << ptr->y << RESET<<std::endl;
            }
            ROS_WARN_STREAM("update_point_id: " << update_point_id << " update_point_x: " << update_point_x << " update_point_y: " << reflectorPanelPoint->y  );


            config.update_up_x_button=false;

        }
        //x down
        if(update_down_x_button&&config.update_point_id!="EMPTY"){

            config.update_point_x=config.update_point_x-update_factor;
            double update_point_x= config.update_point_x;
            std::cout<<BLUE << "更新 point 数据 update_down_x_button  " << RESET<<std::endl;

            //查找类ReflectorPanelPoint对象中的数据
            std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint;
            bool  getInstancefig = ReflectorPanelPoint::getInstance(update_point_id,reflectorPanelPoint);
            if(!getInstancefig){
                std::cout <<RED<< "error dynamic_reconfigure_callback Update a position..." <<"reflectorPanelPoint is empty key： " << update_point_id<<" "<<RESET<<std::endl;
                return;
            }
//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
            reflectorPanelPoint->isMatchok_=-1;
            reflectorPanelPoint->currentIsMatchok_=-1;
            reflectorPanelPoint->isAssignment_=-1;//1:不可以赋值 ，-1：可以赋值
            reflectorPanelPoint->x=update_point_x;//更新数据应为是指针数据，所以reflectorPanelPointAllMaps_数据也会跟着改
            reflectorPanelPoint->isAssignment_=1;

//            pointsSaveMap_.clear();
//            // 函数用于清空文件中的所有数据
//            clearFile( savePointFilePath_);
//            //todo 写入json文件 储存反光板数据到json
//            savePosintsWithJsonFile(reflectorPanelPoints_vector_);

            //更改集合中的数据
            reflectorPanelPointAllMaps_[reflectorPanelPoint->id]=reflectorPanelPoint;
            // 显示一个的反光柱,只显示一次
            fanguangbanShowOneMaker( reflectorPanelPoint);
            for (const auto& ptr : reflectorPanelPoints_vector_) {
                if(ptr->id==update_point_id)
                    std::cout<<GREEN << "UPDATE SUCCESS POINT ID: " << ptr->id<< ", X: " << ptr->x << ", Y: " << ptr->y << RESET<<std::endl;
            }
            ROS_WARN_STREAM("update_point_id: " << update_point_id << " update_point_x: " << update_point_x << " update_point_y: " << reflectorPanelPoint->y  );


            config.update_down_x_button=false;

        }
        //-----------------------
        // y up
        if(update_up_y_button&&config.update_point_id!="EMPTY"){

            config.update_point_y=config.update_point_y+update_factor;
            double update_point_y= config.update_point_y;
            std::cout<<BLUE << "更新 point 数据 update_up_y_button " << RESET<<std::endl;
            //查找类ReflectorPanelPoint对象中的数据
            std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint;
            bool  getInstancefig = ReflectorPanelPoint::getInstance(update_point_id,reflectorPanelPoint);
            if(!getInstancefig){
                std::cout <<RED<< "error dynamic_reconfigure_callback Update a position..." <<"reflectorPanelPoint is empty key： " << update_point_id<<" "<<RESET<<std::endl;
                return;
            }
//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
            reflectorPanelPoint->isMatchok_=-1;
            reflectorPanelPoint->currentIsMatchok_=-1;
            reflectorPanelPoint->isAssignment_=-1;//1:不可以赋值 ，-1：可以赋值
            reflectorPanelPoint->y=update_point_y;//更新数据应为是指针数据，所以reflectorPanelPointAllMaps_数据也会跟着改
            reflectorPanelPoint->isAssignment_=1;

//            pointsSaveMap_.clear();
//            // 函数用于清空文件中的所有数据
//            clearFile( savePointFilePath_);
//            //todo 写入json文件 储存反光板数据到json
//            savePosintsWithJsonFile(reflectorPanelPoints_vector_);

            //更改集合中的数据
            reflectorPanelPointAllMaps_[reflectorPanelPoint->id]=reflectorPanelPoint;
            // 显示一个的反光柱,只显示一次
            fanguangbanShowOneMaker( reflectorPanelPoint);
            for (const auto& ptr : reflectorPanelPoints_vector_) {
                if(ptr->id==update_point_id)
                    std::cout<<GREEN << "UPDATE SUCCESS POINT ID: " << ptr->id<< ", X: " << ptr->x << ", Y: " << ptr->y << RESET<<std::endl;
            }

            ROS_WARN_STREAM("update_point_id: " << update_point_id << " update_point_x: " << reflectorPanelPoint->x << " update_point_y: " << update_point_y  );

            config.update_up_y_button=false;

        }
        //y down
        if(update_down_y_button&&config.update_point_id!="EMPTY"){
            config.update_point_y=config.update_point_y-update_factor;
            double update_point_y= config.update_point_y;
            std::cout<<BLUE << "更新 point 数据 update_down_y_button " << RESET<<std::endl;



            //查找类ReflectorPanelPoint对象中的数据
            std::shared_ptr<ReflectorPanelPoint> reflectorPanelPoint;
            bool  getInstancefig = ReflectorPanelPoint::getInstance(update_point_id,reflectorPanelPoint);
            if(!getInstancefig){
                std::cout <<RED<< "error dynamic_reconfigure_callback Update a position..." <<"reflectorPanelPoint is empty key： " << update_point_id<<" "<<RESET<<std::endl;
                return;
            }
//-1：未开始匹配  0：失去匹配关系  1:匹配上了  2：预测下一次匹配的点
            reflectorPanelPoint->isMatchok_=-1;
            reflectorPanelPoint->currentIsMatchok_=-1;
            reflectorPanelPoint->isAssignment_=-1;//1:不可以赋值 ，-1：可以赋值
            reflectorPanelPoint->y=update_point_y;//更新数据应为是指针数据，所以reflectorPanelPointAllMaps_数据也会跟着改
            reflectorPanelPoint->isAssignment_=1;


//            pointsSaveMap_.clear();
//            // 函数用于清空文件中的所有数据
//            clearFile( savePointFilePath_);
//            //todo 写入json文件 储存反光板数据到json
//            savePosintsWithJsonFile(reflectorPanelPoints_vector_);

            //更改集合中的数据
            reflectorPanelPointAllMaps_[reflectorPanelPoint->id]=reflectorPanelPoint;
            // 显示一个的反光柱,只显示一次
            fanguangbanShowOneMaker( reflectorPanelPoint);
            for (const auto& ptr : reflectorPanelPoints_vector_) {
                if(ptr->id==update_point_id)
                    std::cout<<GREEN << "UPDATE SUCCESS POINT ID: " << ptr->id<< ", X: " << ptr->x << ", Y: " << ptr->y << RESET<<std::endl;
            }

            ROS_WARN_STREAM("update_point_id: " << update_point_id << " update_point_x: " << reflectorPanelPoint->x << " update_point_y: " << update_point_y  );
            config.update_down_y_button=false;

        }
        //---------------------------------------------------------------------
        //动态更新反光板参数  isupdate_point=false: 更新数据  isupdate_point=true:  更新数据
        if(isupdate_point){
            std::cout<<BLUE << "更新 point 数据 isupdate_point = true  " << RESET<<std::endl;
            config.isupdate_point=false;
            if(reflectorPanelPointAllMaps_.size()>0){
                reflectorPanelPoints_vector_.clear();
                for (const auto &entry : reflectorPanelPointAllMaps_) {
                    // entry.first 是键（key），即std::string类型，代表反射板的标识符或名称
                    const std::string &panelName = entry.first;
                    // entry.second 是值（value），即std::shared_ptr<ReflectorPanelPoint>类型，指向反射板点的智能指针
                    const std::shared_ptr<ReflectorPanelPoint> &panelPoint = entry.second;
                    addInsertNewPointToReflectorPanelPoints_vector(reflectorPanelPoints_vector_,panelPoint);
                    // 现在可以访问或操作panelPoint指向的对象了
                    // 例如，打印反射板点的一些信息
                    std::cout << "Panel Name: " << panelName << ", Point Info: ";
                    if (panelPoint) { // 检查智能指针是否有效
                        std::cout << panelPoint; // 假设ReflectorPanelPoint类有一个printInfo()成员函数
                    } else {
                        std::cout << "nullptr";
                    }
                    std::cout << std::endl;
                }

                pointsSaveMap_.clear();
                // 函数用于清空文件中的所有数据
                clearFile( savePointFilePath_);
                //todo 写入json文件 储存反光板数据到json
                savePosintsWithJsonFile(reflectorPanelPoints_vector_);

                // 显示所有的反光柱
                fanguangbanShowAllMaker(reflectorPanelPointAllMaps_);

                for (const auto& ptr : reflectorPanelPoints_vector_) {
                    if(ptr->id==update_point_id)
                        std::cout<<GREEN << "UPDATE SUCCESS POINT ID: " << ptr->id<< ", X: " << ptr->x << ", Y: " << ptr->y << RESET<<std::endl;
                }
            }
        }

        // 显示所有的反光柱
        if(isFirtFanguangbanShowAllMaker_&&reflectorPanelPointAllMaps_.size()>0){
            isFirtFanguangbanShowAllMaker_= false;
            config.isFirtFanguangbanShowAllMaker=isFirtFanguangbanShowAllMaker_;
            // 显示所有的反光柱
            fanguangbanShowAllMaker(reflectorPanelPointAllMaps_);
        }

        // 显示所有的反光柱
//        if(reflectorPanelPointAllMaps_.size()>0){
//            fanguangbanShowAllMaker(reflectorPanelPointAllMaps_);
//        }



//        =config.update_point_id="EMPTY";
//        =config.update_point_x=0.0;
//        =config.update_point_y=0.0;

        //帮我打印所有参数
//        ROS_WARN_STREAM("end 参数服务器： set dynamic_reconfigure" );
//        ROS_WARN_STREAM("multiple_lidar_calibration_x_: " << multiple_lidar_calibration_x_ << " multiple_lidar_calibration_y_: " << multiple_lidar_calibration_y_ << " multiple_lidar_calibration_z_: " << multiple_lidar_calibration_z_
//                                                          << " multiple_lidar_calibration_rx_: " << multiple_lidar_calibration_rx_ << " multiple_lidar_calibration_ry_: " << multiple_lidar_calibration_ry_ << " multiple_lidar_calibration_rz_: " << multiple_lidar_calibration_rz_  );

    }catch (std::exception e){
        ROS_ERROR("ERROR 参数服务器报错： dynamic_reconfigure " );
    }


    last_config_ = config;
}



// 回调函数，处理接收到的消息 /home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/src/test/test52.cpp
void resultGetResultMapvalueCallback(const fanguangbantrilateration::matchcostmapwithscan_result::ConstPtr& msg) {

    // ROS_INFO("resultGetResultMapvalueCallback Received match costmap with scan result:");
    result_Mapvalue_=msg->result_Mapvalue;
    // ROS_INFO("当前地图的代价值 result_Mapvalue: (%f)",msg->result_Mapvalue);
    if(result_Mapvalue_==0){
        std::cout <<GREEN <<  "当前 base_link所在 地图的代价值 13767 resultGetResultMapvalueCallback  result_Mapvalue_：" << result_Mapvalue_<< " ,自由区域"<<RESET<< std::endl;

    }else if(result_Mapvalue_==100){
        std::cout <<RED <<  "当前 base_link所在 地图的代价值 13767 resultGetResultMapvalueCallback  result_Mapvalue_：" << result_Mapvalue_<< " ,障碍物,或地图轮廓"<<RESET<< std::endl;
    }else if(result_Mapvalue_==-1){
        std::cout <<YELLOW <<  "当前 base_link所在 地图的代价值 13767 resultGetResultMapvalueCallback  result_Mapvalue_：" << result_Mapvalue_<< " ,未知区域"<<RESET<< std::endl;
    }

    resultGetResultMapvalueCallback_startTime_ = std::chrono::high_resolution_clock::now();


}
// 回调函数，处理接收到的消息 /home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/src/test/test52.cpp

void resultGetCalculateScanMapRatioCallback(const fanguangbantrilateration::matchcostmapwithscan_result::ConstPtr& msg) {
    ROS_INFO("resultGetCalculateScanMapRatioCallback Received match costmap with scan result:");
    is_ratio_= msg->isCalculateScanMapRatio;//是否满足匹配条件
    matching_points_=msg->resultmatching_points_size;
    scan_points_map_size_=msg->scan_points_map_size;
    total_points_size_=msg->resulttotal_points_size;//雷达点数据量的有效数据量
    double ratio =0;
    //if(scan_points_map_size_!=0|| total_points_size_!=0){
    if(scan_points_map_size_!=0){
        //scan_points_map_size_= true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，
        // 需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。
        if(is_use_scan_points_map_size_){
            ratio = static_cast<double>(matching_points_) / scan_points_map_size_;//计算所有雷达数据量与地图轮廓的匹配率
        }else{
            ratio = static_cast<double>(matching_points_) / total_points_size_;//计算雷达点数据量的有效数据量与地图轮廓的匹配率
        }


    }

    std::cout <<GREEN <<  "计算雷达点在地图中的匹配度 13781 resultGetCalculateScanMapRatioCallback 匹配上的雷达数据量 matching_points_：" << matching_points_<<  " ,雷达数量 scan_points_map_size_: " << scan_points_map_size_  <<  " ,雷达点数据量的有效数据量 total_points_size_: " << total_points_size_  <<RESET<< std::endl;
    if(ratio>=matchingcostma_threshold_){
        std::cout <<GREEN <<  "计算雷达点在地图中的匹配度 13782 resultGetCalculateScanMapRatioCallback 匹配上的雷达数有效比例 ratio：" << ratio<< ", 比例 阀值 matchingcostma_threshold_:  " << matchingcostma_threshold_  <<RESET<< std::endl;

    }else{
        std::cout <<RED <<  "计算雷达点在地图中的匹配度 13783 resultGetCalculateScanMapRatioCallback 匹配上的雷达数有效比例 ratio：" << ratio<< ", 比例 阀值 matchingcostma_threshold_:  " << matchingcostma_threshold_  <<RESET<< std::endl;
    }

    resultGetCalculateScanMapRatioCallback_startTime_ = std::chrono::high_resolution_clock::now();

}



//============================================================================================================

//todo 把反光柱计算出来的数据放入队列中，然后通过 odomCallback 进行处理

// 存储上一次的位姿和时间
geometry_msgs::Pose last_pose;
ros::Time last_time;
bool has_last_pose = false;  // 用于标记是否已经有上一帧数据

// 计算欧氏距离
double calculateDistance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) {
    return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}

// 计算角度差
double calculateYawDifference(const geometry_msgs::Quaternion& q1, const geometry_msgs::Quaternion& q2) {
    double yaw1 = tf::getYaw(q1);
    double yaw2 = tf::getYaw(q2);
    return yaw2 - yaw1;
}

// 辅助函数：yaw角的正则化
double normalizeYaw(double yaw) {
    while(yaw > M_PI) yaw -= 2*M_PI;
    while(yaw < -M_PI) yaw += 2*M_PI;
    return yaw;
}

// 回调函数，用于接收里程计数据并计算速度
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {







    // 获取当前时间
    ros::Time current_time = ros::Time::now();

    // 如果有上一帧数据，则计算速度
    if (has_last_pose) {
        double delta_t = (current_time - last_time).toSec();  // 计算时间差


        if (optimizePoseWithTrilaterationPose_queue_.empty())
            return;


        // 当前里程计数据
        tf::Transform odomTbase_link_now_transform;
        odomTbase_link_now_transform.setOrigin(tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z));  // 设置平移
        // 将四元数转换为 tf::Quaternion
        tf::Quaternion tf_quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
        odomTbase_link_now_transform.setRotation(tf_quaternion);  // 设置旋转


        // 上一次的里程计数据
        tf::Transform last_pose_odomTbase_link_now_transform;
        last_pose_odomTbase_link_now_transform.setOrigin(tf::Vector3(last_pose.position.x, last_pose.position.y, last_pose.position.z));  // 设置平移
        // 将四元数转换为 tf::Quaternion
        tf::Quaternion tf_last_pose_quaternion(last_pose.orientation.x, last_pose.orientation.y, last_pose.orientation.z, last_pose.orientation.w);
        last_pose_odomTbase_link_now_transform.setRotation(tf_last_pose_quaternion);  // 设置旋转



        OptimizePoseWithTrilaterationPose   optimizePoseWithTrilaterationPose  = optimizePoseWithTrilaterationPose_queue_.back();  // 取出最后一个元素，为反光柱计算得到的定位数据
        // 已经有了当前时间
        ros::Time optimizePoseWithTrilaterationPose_current_time = optimizePoseWithTrilaterationPose.stamp;

        //显示
        std::vector<std::tuple<std::shared_ptr<ReflectorPanelPoint>,std::shared_ptr<ReflectorPanelPoint>> > indices=optimizePoseWithTrilaterationPose.indices;
        std::vector<Point> circle=optimizePoseWithTrilaterationPose.circle;
        std::string matchReflectorPaneIds = optimizePoseWithTrilaterationPose.matchReflectorPaneIds;





        //todo 反光柱计算得到的数据 mapTbase_link
        tf::Transform          mapTbase_link_transform_min = optimizePoseWithTrilaterationPose.mapTbase_transform_min;//反光柱计算得到的数据 mapTbase_link


        tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_link_transform_min, ros::Time::now(), map_frame_id_, "111111111111111111111111111111111111111"));



        //    通过以上代码中 当前里程计数据odomTbase_link_now_transform, 反光柱计算得到的定位数据 mapTbase_link_transform_min，以及 反光柱计算时里程数据 last_odomTbase_link_transform，计算当前mapTodom ， mapTbase_link数据;
        //    我项目的坐标系是这样的： map->odom->base_link->laser
        tf::StampedTransform   last_laser_to_odom_transform = optimizePoseWithTrilaterationPose.last_laser_to_odom_transform;//odomTlaser
        tf::StampedTransform   last_mapTlaser_transform = optimizePoseWithTrilaterationPose.last_mapTlaser_transform;

        tf::Transform  last_mapTodom_transform    =  last_mapTlaser_transform*last_laser_to_odom_transform.inverse();

        //todo 反光柱计算使用的 odomTbase_link,使用的里程数据
        tf::StampedTransform   last_odomTbase_link_transform = optimizePoseWithTrilaterationPose.last_odomTbase_link_transform;
        // 获取 tf::Quaternion
        tf::Quaternion  last_base_link_to_odom_tf_quaternion = last_odomTbase_link_transform.getRotation();
        // 转换为 geometry_msgs::Quaternion
        geometry_msgs::Quaternion last_base_link_to_odom_geom_quaternion;
        last_base_link_to_odom_geom_quaternion.x = last_base_link_to_odom_tf_quaternion.x();
        last_base_link_to_odom_geom_quaternion.y = last_base_link_to_odom_tf_quaternion.y();
        last_base_link_to_odom_geom_quaternion.z = last_base_link_to_odom_tf_quaternion.z();
        last_base_link_to_odom_geom_quaternion.w = last_base_link_to_odom_tf_quaternion.w();


        // 当前里程计数据与上一次里程计数据之间的变换（用的是反光柱带来的last_odomTbase ）
        // tf::Transform  now_odomTlast_odom_transform = odomTbase_link_now_transform*last_odomTbase_link_transform.inverse();
        // 当前里程计数据与上一次里程计数据之间的变换（用的是odomCallback订阅得到的上一次 last_odomTbase ）
        //  tf::Transform  now_odomTlast_odom_transform = odomTbase_link_now_transform*last_pose_odomTbase_link_now_transform.inverse();
        //======================================================================================
        //当前角度
        double current_yaw = tf::getYaw(msg->pose.pose.orientation);
        //【增量数据】 当前位置与上一次位置的角度差（odom）
        // double delta_yaw =normalizeYaw( calculateYawDifference(last_base_link_to_odom_geom_quaternion, msg->pose.pose.orientation));
        double delta_yaw =calculateYawDifference(last_base_link_to_odom_geom_quaternion, msg->pose.pose.orientation);////last_odomTbase_link_transform 反光柱计算使用的 odomTbase_link,使用的里程数据
        //上一次的角度（上一次通过运动模型计算里程计所用的角度就是，current_yaw-delta_yaw）
        //double previous_yaw =  current_yaw-delta_yaw;
        //上一次的角度
        double previous_yaw = tf::getYaw(last_base_link_to_odom_geom_quaternion);

        //【增量数据】 当前位置与上一次位置的位置差 （odom）
        double delta_x  =  msg->pose.pose.position.x-last_odomTbase_link_transform.getOrigin().getX();//last_odomTbase_link_transform 反光柱计算使用的 odomTbase_link,使用的里程数据
        double delta_y  =  msg->pose.pose.position.y-last_odomTbase_link_transform.getOrigin().getY();
        //todo 计算得到当前的 v_x，v_y 速度
        //        double v_x = (delta_x / delta_t) * cos(previous_yaw) + (delta_y / delta_t) * sin(previous_yaw);
        //        double v_y = -(delta_x / delta_t) * sin(previous_yaw) + (delta_y / delta_t) * cos(previous_yaw);

        double v_x = (delta_x / delta_t) * cos(previous_yaw) + (delta_y / delta_t) * sin(previous_yaw);
        double v_y = -(delta_x / delta_t) * sin(previous_yaw) + (delta_y / delta_t) * cos(previous_yaw);

        //todo 计算得到当前的 v_yaw 角速度
        double v_yaw = delta_yaw / delta_t;

        //todo ----------预测里程增量----------------------------有用
        // double  delta_t1=delta_t;
        delta_t=delta_t/2.2;
        delta_x = (v_x * cos(current_yaw) - v_y * sin(current_yaw)) * delta_t;//时间的长度需要自己调整，与实际定位数据匹配
        delta_y = (v_x * sin(current_yaw) + v_y * cos(current_yaw)) * delta_t;
        delta_yaw = v_yaw * delta_t;

        //计算得到当前 应用增量后的里程数据
        tf::Transform odomTbase_link_delta_now_transform;
        odomTbase_link_delta_now_transform.setOrigin(tf::Vector3(odomTbase_link_now_transform.getOrigin().getX()+delta_x, odomTbase_link_now_transform.getOrigin().getY()+delta_y, 0.0));  // 设置平移
        // 1. 计算四元数
        tf::Quaternion delta_rotation;
        delta_rotation.setRPY(0, 0, current_yaw-delta_yaw);  // 绕Z轴旋转的四元数 current_yaw+delta_yaw
        // 2. 设置旋转到 odomTbase_link_delta_now_transform 中
        odomTbase_link_delta_now_transform.setRotation(delta_rotation);//

        // odomTbase_link_delta_now_transform=odomTbase_link_now_transform;


        //==============================================================================================


        // 计算 mapTodom
        tf::Transform mapTodom_transform = mapTbase_link_transform_min * last_odomTbase_link_transform.inverse();//last_odomTbase_link_transform 反光柱计算使用的 odomTbase_link,使用的里程数据
        // tf::Transform mapTodom_transform = mapTbase_link_transform_min * last_odomTbase_link_transform.inverse();//odomTbase_link_delta_now_transform 是odom的数据


        //未使用应用增量数据后计算的mapTbase_link
        // tf::Transform mapTbase_link_now_transform = mapTodom_transform * odomTbase_link_now_transform;
        //使用应用增量数据后计算的mapTbase_link
        tf::Transform mapTbase_link_now_transform = mapTodom_transform *odomTbase_link_delta_now_transform;

        // 计算最新的 mapTodom
        // mapTodom_transform = mapTbase_link_now_transform * odomTbase_link_now_transform.inverse();



        // 计算当前的 mapTbase_link
        //  tf::Transform mapTbase_link_now_transform1 = mapTodom_transform * odomTbase_link_now_transform;

        tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTbase_link_now_transform, ros::Time::now(), map_frame_id_, "222222222222222222222222222222222222222"));


        //---------------------------------------------------------------------
        // 使用remove_if移除时间戳小于当前时间的元素
        optimizePoseWithTrilaterationPose_queue_.erase(
                std::remove_if(
                        optimizePoseWithTrilaterationPose_queue_.begin(),
                        optimizePoseWithTrilaterationPose_queue_.end(),
                        [optimizePoseWithTrilaterationPose_current_time](const OptimizePoseWithTrilaterationPose& pose) {
                            return pose.stamp <= optimizePoseWithTrilaterationPose_current_time;
                        }
                ),
                optimizePoseWithTrilaterationPose_queue_.end()
        );

        //---------------------------------------------------------------------


        tf::StampedTransform base_link_to_map_transform;//mapTbase_link
        try {
            //todo  mapTbase_link
            tf_listener_new_->waitForTransform(map_frame_id_, base_link_frame_id_,ros::Time(0), ros::Duration(1.0));
            tf_listener_new_->lookupTransform(map_frame_id_, base_link_frame_id_, ros::Time(0), base_link_to_map_transform);
            base_link_to_map_transform_=base_link_to_map_transform;
        } catch (tf::TransformException& ex) {
            ROS_ERROR(" odomCallback  %s", ex.what());
            return;
        }


        //todo  判断是否是原地旋转 , 原地转到的 位移在0.0001-0.0003之间，角度大于0.01  , 行走时的位移在0.01-0.02之间，角度大于等于0.0001  .previous_base_link_to_odom_transform_  base_link_to_odom_transform_；time_log_fig_==12
        bool is_in_place_rotation = CheckInPlaceRotation( odomTbase_link_now_transform, last_pose_odomTbase_link_now_transform,angle_threshold_,distance_threshold_);

        double variance_min_yaw_lerance=variance_min_yaw_lerance_, variance_min_pose_lerance=variance_min_pose_lerance_;

        if(is_in_place_rotation){//true 原地旋转
                  ////is_start_place_rotation=true 后会加载 rotate_in_place_variance_multiplier 在原地旋转时放大定位精度，从而减少定位的抖动带来的移动机器人反复调整位置.
                variance_min_pose_lerance=variance_min_pose_lerance*rotate_in_place_variance_multiplier_;
                variance_min_yaw_lerance=variance_min_yaw_lerance*rotate_in_place_variance_multiplier_;
        }




        //scan_CB_new1 计算方差  35.8329
        double varianceYaw_min = calculateVarianceYaw(mapTbase_link_now_transform, base_link_to_map_transform);

        double variancePose_min = calculateVariancePose(mapTbase_link_now_transform, base_link_to_map_transform);

        //发布评估数据
        std::string resultType = "odomCallback";
        std::string resultChineseMessage ="这里判断计算出来的定位与当前tf的定位误差值，小于"+std::to_string(variance_min_pose_lerance_)+" 位置容忍阀值与小于 "+std::to_string(variance_min_yaw_lerance_)+" 角度位置容忍阀值就不更新定位，减少定位波动（波动会影响机器人运动的卡顿情况）。需要大于一定阀值时才会使用新的定位数据。 [发布定位数据]";
        std::string resultENMessage = "Here, the calculated localization error compared to the current tf localization is smaller than the position tolerance threshold of " + std::to_string(variance_min_pose_lerance_) + " and the yaw tolerance threshold of " + std::to_string(variance_min_yaw_lerance_) + ". In this case, localization is not updated to reduce localization fluctuations (which may cause jerky robot motion). The new localization data will only be used if it exceeds a certain threshold. [Publishing localization data]";

        std::string className = __FILE__; // 更改为实际的文件名
        int classLine = __LINE__; // 更改为实际的行号

        std::string message = "满足 variancePose_min,varianceYaw_min 精度阀值，不需要触发反光柱定位纠正定位误差。";
        // 输出执行时间
        std::cout <<message << std::endl;
        resultENMessage = "满足 variancePose_min,varianceYaw_min 精度阀值，不需要触发反光柱定位纠正定位误差。";
        className = __FILE__; // 更改为实际的文件名
        classLine = __LINE__; // 更改为实际的行号
        geometry_msgs::Pose mapTbase_link;
        double mapTbase_link_yaw=0.0; // ,matchReflectorPaneIds,mapTbase_link,mapTbase_link_yaw
        // /trilateration_time_log
        std::cout << resultChineseMessage <<std::endl;



        // if(varianceYaw_min < variance_min_yaw_lerance_ &&  variancePose_min < variance_min_pose_lerance_ && isTrilateration_with_yawFig_ && !firstTrilateration_with_yawFig_){
        if(varianceYaw_min < variance_min_yaw_lerance &&  variancePose_min < variance_min_pose_lerance ){

            if(is_in_place_rotation){
                message = "满足 variancePose_min,varianceYaw_min 精度阀值，不需要触发反光柱定位纠正定位误差。**当前移动机器人处于原地旋转状态** ";
            }else{
                message = "满足 variancePose_min,varianceYaw_min 精度阀值，不需要触发反光柱定位纠正定位误差。**当前移动机器人处于原地旋转状态** ";
            }
            resultENMessage = "满足 variancePose_min,varianceYaw_min 精度阀值，不需要触发反光柱定位纠正定位误差。";
            resultChineseMessage =message+ "[发布定位数据]";
            //   if(varianceYaw_min < accuracy_variance_yaw_threshold_ &&  variancePose_min < accuracy_variance_pose_threshold_ ){



            if(time_log_fig_==22) {//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....
                ///trilateration_time_log
                publishTrilaterationLog( delta_t, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_
                        ,mapTbase_link_now_transform,base_link_to_map_transform,last_pose_odomTbase_link_now_transform,odomTbase_link_now_transform
                        ,variancePose_min,varianceYaw_min,variance_min_pose_lerance,variance_min_yaw_lerance,optimizePoseWithTrilaterationPose_queue_.size());


            }

            //14136 显示             //todo scan_CB_new1 得到匹配上的数据，方差最小的数据 【把匹配上的数据加入matchokReflectorPanelPointEnd_Unordered_map_ 并 赋值后显示出来】matchokReflectorPanelPointEnd_Unordered_map_.push_back. 9916
            updateMatchedReflectors(indices,circle,matchReflectorPaneIds);
            return ;
        }else{

            if(is_in_place_rotation){
                message = "不满足 variancePose_min,varianceYaw_min 精度阀值，需要触发反光柱定位纠正定位误差。**当前移动机器人处于原地旋转状态** ";
            }else{
                message = "不满足 variancePose_min,varianceYaw_min 精度阀值，需要触发反光柱定位纠正定位误差。**当前移动机器人处于原地旋转状态** ";
            }
            message = "不满足 variancePose_min,varianceYaw_min 精度阀值，需要触发反光柱定位纠正定位误差。";
            matchReflectorPaneIds="EMPTY";
            // 输出执行时间
            std::cout <<message << std::endl;
            resultChineseMessage =message+ "[发布定位数据]";
            resultENMessage = "不满足variancePose_min,varianceYaw_min精度阀值，需要触发反光柱定位纠正定位误差。";
            if(time_log_fig_==22) {
                ///trilateration_time_log
                publishTrilaterationLog( delta_t, resultType, resultChineseMessage, resultENMessage, className, classLine,time_log_fig_
                        ,mapTbase_link_now_transform,base_link_to_map_transform,last_pose_odomTbase_link_now_transform,odomTbase_link_now_transform
                        ,variancePose_min,varianceYaw_min,variance_min_pose_lerance,variance_min_yaw_lerance,optimizePoseWithTrilaterationPose_queue_.size());

            }

        }// if(varianceYaw_min < variance_min_yaw_lerance_ &&  variancePose_min < variance_min_pose_lerance_ && isTrilateration_with_yawFig_ && !firstTrilateration_with_yawFig_)


        //是否启动判断是否是原地旋转功能，启动后 原地旋转 不会更新定位数据
        if(is_start_place_rotation_){
            //todo  判断是否是原地旋转 , 原地转到的 位移在0.0001-0.0003之间，角度大于0.01  , 行走时的位移在0.01-0.02之间，角度大于等于0.0001  .previous_base_link_to_odom_transform_  base_link_to_odom_transform_；time_log_fig_==12
            bool is_in_place_rotation = CheckInPlaceRotation( odomTbase_link_now_transform, last_pose_odomTbase_link_now_transform,angle_threshold_,distance_threshold_);
            /// //bool is_in_place_rotation = CheckInPlaceRotation(previous_transform, base_link_to_map_transform_);
            if(is_in_place_rotation&&firstTrilateration_with_yawFig_){ //原地旋转 就不使用反光板数据计算定位了。
                previous_base_link_to_odom_transform_=base_link_to_odom_transform_;
                previous_base_link_to_map_transform_ =base_link_to_map_transform_;
                //经典三边定位最小二乘法 每次计算得到的 mapTbase_link 的开始时间
                trilateration_mapTbase_link_pose_startTime_ = std::chrono::high_resolution_clock::now();
                std::cout << "正在原地旋转，不用反光板数据计算定位，yaw_threshold_=" <<variance_yaw_threshold_ <<std::endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
                return ;//原地旋转
            }

        }



        // todo 10. isSendTf_
        if(isSendTf_|| test_num_!=10) {//发布坐标关系

            isRunProcessQueueFig_ =false;// 11658 控制 processQueue tf 发布
            std::this_thread::sleep_for(std::chrono::milliseconds(1));

            tf_last_odom_to_map_transform_=mapTodom_transform;//laser_mapTodom_transform
            //scan_CB_new1
            tf_broadcaster_new_->sendTransform(tf::StampedTransform(mapTodom_transform, ros::Time::now(), map_frame_id_, odom_frame_id_));
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
            isRunProcessQueueFig_ =true;
        }


        //14136 显示             //todo scan_CB_new1 得到匹配上的数据，方差最小的数据 【把匹配上的数据加入matchokReflectorPanelPointEnd_Unordered_map_ 并 赋值后显示出来】matchokReflectorPanelPointEnd_Unordered_map_.push_back. 9916
        updateMatchedReflectors(indices,circle,matchReflectorPaneIds);
    }//if (has_last_pose)


    // 输出计算结果
    ROS_INFO("************************88optimizePoseWithTrilaterationPose_queue_.size(): %i ***************************************", optimizePoseWithTrilaterationPose_queue_.size());



    // 更新上一帧位姿和时间
    last_pose = msg->pose.pose;
    last_time = current_time;
    has_last_pose = true;

}





//iswileTure_
//ctrl + c
void mySigintHandler(int sig) {
    std::cout << "ctrl c  sig: " << sig << std::endl;
//    if (currency != NULL)
//        delete currency;
    iswileTure_=false;
    ros::shutdown();
}
//   rosrun fanguangbantrilateration fanguangbantrilateration_read_time_log_node
//   roslaunch fanguangbantrilateration trilateration_new.launch
int main(int argc, char *argv[]) {
    setlocale(LC_ALL, "");  //中文 _test42_node
    ros::init(argc, argv, "fanguangbantrilateration_read_time_log_node");
    ros::NodeHandle nh;
    ros::NodeHandle nnh("~");
    std::string savePointFileName="allpoint.txt";//储存所有反光板点位到txt
    std::string calculateFileName="calculate.txt";//储存计算好的反光板特征数数据到txt，2个点计算一条直线
    std::string scanIntensitiesFileName="scanIntensities.txt";//储存雷达强度数据对应表
    std::string  mapTbaselinkPoseFileName="mapTbaselinkPose.txt";//储存移动机器人的位置
    signal(SIGINT, mySigintHandler);


    nnh.param<std::string>("reflectormarker_marker_toptic_name", reflectormarker_marker_toptic_name_, "/reflectormarker_marker");
    nnh.param<std::string>("scan_toptic_name", scan_toptic_name_, "/scan");
    nnh.param<std::string>("get_points_toptic_name", get_points_toptic_name_, "/clicked_point");
    nnh.param<std::string>("initialpose_toptic_name", initialpose_toptic_name_, "/initialpose");
    nnh.param<std::string>("show_marker_toptic_name", show_marker_toptic_name_, "/trilateration_marker");
    nnh.param<std::string>("show_all_marker_toptic_name", show_all_marker_toptic_name_, "/trilateration_all_marker");
    nnh.param<std::string>("show_all_current_marker_toptic_name", show_all_current_marker_toptic_name_, "/trilateration_all_current_marker");
    nnh.param<std::string>("show_circle_points_toptic_name", show_circle_points_toptic_name_, "/trilateration_circle_points");
    nnh.param<std::string>("service_name", service_name_, "/trilateration_service");
    nnh.param<std::string>("reflectormarker_path_marker_toptic_name", reflectormarker_path_marker_toptic_name_, "/reflectormarker_path_marker");//发布反光柱与雷达的连线
    nnh.param<int>("points_size_threshold", points_size_threshold_, 2);////点的阀值，点云聚类，分割出各个反光柱。至少连续三个点强度超过   强度阀值   即默认为反光柱,单独点滤除

    nnh.param<int>("findPolygonIndices_threshold", findPolygonIndices_threshold_, 2);

    //【过滤掉反光柱两俩之间距离太近（在 filter_tow_ont_distance_threshold_ 阀值内就剔除）的点（有可能是数据波动产生的）】
    nnh.param<double>("filter_tow_ont_distance_threshold", filter_tow_ont_distance_threshold_, 2);

    //评估性能，发布性能评估数据
    nnh.param<std::string>("time_log_topic_name", time_log_topic_name_, "/trilateration_time_log");

    nnh.param<std::string>("map_frame_id", map_frame_id_, "map");
    nnh.param<std::string>("base_frame_id", base_link_frame_id_, "base_link");
    nnh.param<std::string>("odom_frame_id", odom_frame_id_, "odom");
    nnh.param<std::string>("laser_frame_id", laser_frame_id_, "laser");//会自动获取scan中的数据。base_scan laser
    nnh.param<std::string>("imu_frame_id", imu_frame_id_, "imu_link");
    //文件目录与文件名称
    nnh.param<std::string>("fileOutPut", fileOutPut_, fileOutPut_);
    nnh.param<std::string>("savePointFileName", savePointFileName, "allpoint.txt");
    nnh.param<std::string>("calculateFileName", calculateFileName, "calculate.txt");
    nnh.param<std::string>("scanIntensitiesFileName", scanIntensitiesFileName, "scanIntensities.txt");
    savePointFilePath_=fileOutPut_+"/"+savePointFileName;
    calculateFilePath_=fileOutPut_+"/"+calculateFileName;
    scanIntensitiesFilePath_=fileOutPut_+"/"+scanIntensitiesFileName;////储存雷达强度数据对应表  5083 改数据
    mapTbaselinkPose_path_=fileOutPut_+"/"+mapTbaselinkPoseFileName;

    nnh.param<int>("end_scan_index", end_scan_index_, 9999999);//雷达扫描结束点索引
    nnh.param<int>("time_log_fig", time_log_fig_, -1);//-1:全部关闭，0：全部打开 ，  -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 ，1：打开第一个，2：打开第二个 3：.....

    nnh.param<int>("test_num", test_num_, -1);//参数服务测试使用，没有实际意思。通过这个参数可以动态的调试代码，把加入到代码调试位置

    nnh.param<int>("time_milliseconds", time_milliseconds_, 100);//匹配上的点的更新速度 ms
    nnh.param<double>("scan_distances_threshole", scan_distances_threshole_, 10.0);//超过这个值为无效数据，雷达有效范围，也在预测范围内，单位m


    nnh.param<double>("mapTbase_link_pos_threshold", mapTbase_link_pos_threshold_, 10.0);//3边定位最小二乘 计算得到的 mapTbase_link  位置阈值
    nnh.param<double>("mapTbase_link_rot_threshold", mapTbase_link_rot_threshold_, 10.0);//3边定位最小二乘 计算得到的  角度阈值，例如10度

    //calculateEdge_threshold_ 计算特征时的距离阀值，计算距离阀值，两点过远就不需要计算了
    nnh.param<double>("calculateEdge_threshold", calculateEdge_threshold_, 5.0);

    //todo 定义哈希函数，map中key的允许误差，key是两个double浮点数
    nnh.param<double>("epsilon", epsilon_, 0.11);//todo  重要数据 误差精度影响map的key搜索，误差不能大于两个反光板直接的距离

    nnh.param<double>("fangaungbanHeight", fangaungbanHeight_, 0.5);//反光板厚度
    nnh.param<double>("fangaungbanWidth", fangaungbanWidth_, 0.10);//反光板宽度
    nnh.param<double>("fangaungbanLength", fangaungbanLength_, 0.50);//0.001反光板高度设为一个很小的值，表示一个平面


    nnh.param<double>("fangaungzhuRadius", fangaungzhuRadius_, 0.045);//反光柱半径
    nnh.param<double>("fangaungzhuHeight", fangaungzhuHeight_, 0.09);//反光柱厚度
    nnh.param<double>("fangaungzhuWidth", fangaungzhuWidth_, 0.09);//反光柱宽度
    nnh.param<double>("fangaungzhuLength", fangaungzhuLength_, 0.50);//反光柱高度


    //如果平均曲率超过阈值，认为是反光柱
    nnh.param<double>("curvature_threshold", curvature_threshold_, 0.01);
    //在阀值距离范围内，判断 curvature_threshold_ 是反光柱还是反光板
    nnh.param<double>("curvature_range_min_threshold", curvature_range_min_threshold_, 1.5);//距离小于阀值有效
    nnh.param<double>("curvature_range_max_threshold", curvature_range_max_threshold_, 2.6);//距离大于阀值有效

    //匹配上的反光板颜色
    nnh.param<double>("a", a_, 0.01);//透明度
    nnh.param<double>("r", r_, 0.01);
    nnh.param<double>("g", g_, 0.01);
    nnh.param<double>("b", b_, 0.01);

    nnh.param<double>("scan_intensitie", scan_intensitie_, 1500.0);//雷达强度值
    nnh.param<double>("intensitie_threshole", intensitie_threshole_, 100.0);//雷达强度阀值
    nnh.param<double>("range_threshole", range_threshole_, 0.1);//雷达扫描距离阀值

    nnh.param<double>("matchok_timeout", matchok_timeout_, 5.0);//反光板匹配上了,超时阈值，就剔除。单位为秒
    nnh.param<double>("edgesthreshold", edgesthreshold_, 0.1);//反光板，柱 的特征匹配的边长特征的阀值
    nnh.param<double>("anglesthreshold", anglesthreshold_, 0.157);//反光板，柱 的特征匹配的两边夹角的阀值


    nnh.param<double>("isCircular_threshold", isCircular_threshold_, 0.1);//判断是否是园，设置一个阈值，根据方差来判断是否近似为圆
    nnh.param<double>("isLine_threshold", isLine_threshold_, 0.1);//判断是否是直线，使用了较小的斜率阈值，使得只要拟合的直线接近水平，就认为这组点云近似于一条直线。



    nnh.param<bool>("isSendTf", isSendTf_, true);//是否发布定位的tf坐标
    nnh.param<bool>("isFirtFanguangbanShowAllMaker", isFirtFanguangbanShowAllMaker_, true);//显示所有的的反光板点位,这个只会显示一次
    nnh.param<bool>("is_trilateration", is_trilateration_, true);//是否使用反光板定位，false时，不使用反光板定位，订阅的scan中会return
    nnh.param<bool>("isSend_publish_circle_pointCloud", isSend_publish_circle_pointCloud_, true);//是否发布过滤后的只有强度数据的点云，雷达扫描到的反光柱轮廓
    nnh.param<bool>("isStartAreAnglesEqualNew", isStartAreAnglesEqualNew_, true);//是否加入对比两线段的夹角作为匹配条件，true：加入，false：不加入
    nnh.param<bool>("isShowFanGuanBanPoint", isShowFanGuanBanPoint_, true);//是否显示反光板点位，一般部署会用到，核对反光板点位是否准确，不准确就动态调参数

    nnh.param<bool>("isShowMatchokReflectorFanGuanBanPoint", isShowMatchokReflectorFanGuanBanPoint_, true);


    nnh.param<bool>("isReadMatchokReflectorPanel", isReadMatchokReflectorPanel_, true);//true：当有匹配上反光板后，优先使用匹配好的反光板数据。false：不使用匹配好的反光板数据，一直只使用反光板特征匹配算法
    nnh.param<bool>("isShowFanGuanBanmarkerPath", isShowFanGuanBanmarkerPath_, true);//是否显示反光板与小车之间的匹配连线

    // 需要分优先顺序，环境中反光板数据上千，上万个选择 isFindReflectorPanelPoints_vector_first_： true，反光板数据上百个isFindReflectorPanelPoints_vector_first_：值随意，最佳选择 isFindReflectorPanelPoints_vector_first_： false。
    nnh.param<bool>("isFindRlectorPanelPointAllUnordered_maps", isFindRlectorPanelPointAllUnordered_maps_, true);
    ////当通过key在map中查不到数据时是否遍历列表集合来查询数据,配合 reflectorPanelPoints_vector_size_ 使用代表需要遍历前面多少数据，reflectorPanelPoints_vector_集合中的数据是安预测可能性排序的，可能性越大排序越靠前
    nnh.param<bool>("isFindReflectorPanelPoints_vector", isFindReflectorPanelPoints_vector_, true);
    ////是匹配时否搜索 reflectorPanelPointAllUnordered_maps中 的所有数据
    nnh.param<bool>("isFindReflectorPanelPoints_vector_first", isFindReflectorPanelPoints_vector_first_, true);
    // 需要分优先顺序，环境中反光板数据上千，上万个选择 isFindReflectorPanelPoints_vector_first_： true，反光板数据上百个isFindReflectorPanelPoints_vector_first_：值随意，最佳选择 isFindReflectorPanelPoints_vector_first_： false。
    nnh.param<int>("reflectorPanelPoints_vector_size", reflectorPanelPoints_vector_size_, 8);
    //是否配合第三方定位算法一起使用，比如 amcl等。true：表示一起使yong
    nnh.param<bool>("isUsingThirdPartyLocalization", isUsingThirdPartyLocalization_, true);

    //记录是否已经通过3边定位成功了一次，第一次定位成功就设置true
    nnh.param<bool>("firstTrilateration_with_yawFig", firstTrilateration_with_yawFig_, false);
    //角度方差 定义一个方差阀值,判断当前通过3边定位算法出来的结果与里程计的方差是否小于阀值
    nnh.param<double>("variance_yaw_threshold", variance_yaw_threshold_, 0.006);
    //满足角度调整阀值，可以设置 variance_yaw_threshold_ 的倍数作为参数
    nnh.param<double>("variance_adjustment_yaw_threshold", variance_adjustment_yaw_threshold_, 0.018);

    //位置方差 定义一个方差阀值,判断当前通过3边定位算法出来的结果与里程计的方差是否小于阀值
    nnh.param<double>("variance_pose_threshold", variance_pose_threshold_, 0.06);
    //满足位置调整阀值，可以设置 variance_pose_threshold_ 的倍数作为参数-
    nnh.param<double>("variance_adjustment_pose_threshold", variance_adjustment_pose_threshold_, 0.3);

    //位置方差 超过这个精度就使用反光柱定位数据进行定位纠正
    nnh.param<double>("variance_min_pose_lerance", variance_min_pose_lerance_, 0.03);
    //角度方差 超过这个精度就使用反光柱定位数据进行定位纠正
    nnh.param<double>("variance_min_yaw_lerance", variance_min_yaw_lerance_, 0.03);


    //匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿,如果score 大于阀值 1.2且小于阀值  等于3.0，表示匹配成功的。 ComputeCurrentPose 10090
    nnh.param<double>("score_min_threshold", score_min_threshold_, 1.2);
    //匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿,如果score 大于阀值 1.2且小于阀值  等于3.0，表示匹配成功的。 ComputeCurrentPose 10090
    nnh.param<double>("score_max_threshold", score_max_threshold_, 3.0);


    //连续出现3次定位失败就重新置为 firstTrilateration_with_yawFig_ = fasle。重新定位
    nnh.param<int>("isNotMin_varianceNum_target", isNotMin_varianceNum_target_, 3);
    //三边定位最小二乘法计算得到的mapTbase_link结果不符合阀值条件的设定的目标超时时间
    nnh.param<double>("targetTrilaterationTimeOut", targetTrilaterationTimeOut_, 0.3);

    //是否启动判断是否是原地旋转功能，启动后 原地旋转 不会更新定位数据
    nnh.param<bool>("is_start_place_rotation", is_start_place_rotation_, false);

    //is_start_place_rotation=true 后会加载 rotate_in_place_variance_multiplier 在原地旋转时放大定位精度，从而减少定位的抖动带来的移动机器人反复调整位置.
    nnh.param<int>("rotate_in_place_variance_multiplier", rotate_in_place_variance_multiplier_, 4);
    //--------------------------------------------------- fanguangbantrilateration/src/matchCostmapWithScan.cpp ----------------------------------------------------------------------------------------------------------

    nnh.param<std::string>("getResultMapvalue_result_toptic_name", getResultMapvalue_result_toptic_name_, "/match_costmap_resultMapvalue");
    nnh.param<std::string>("getCalculateScanMapRatio_result_toptic_name", getCalculateScanMapRatio_result_toptic_name_, "/match_costmap_calculateScanMapRatio");

    //超时说明没有订阅到coatmap的数据
    nnh.param<double>("matchcostmapwithscanTimeOut", matchcostmapwithscanTimeOut_, 3.0);

    //是否发布数据 从另一类中发布，从另一类 fanguangbantrilateration/src/matchCostmapWithScan.cpp
    nnh.param<bool>("isPublish_matchcostmapwithscan_mssage", isPublish_matchcostmapwithscan_mssage_, false);
    nnh.param<std::string>("match_costmap_with_scan_service_name", match_costmap_with_scan_service_name_, "/match_costmap_with_scan_service");

    nnh.param<int>("free_cell", free_cell_, 0);// 0代表空闲
    nnh.param<int>("occupied_cell", occupied_cell_, 100);//100代表占用
    nnh.param<int>("unknown_cell", unknown_cell_, -1);// -1代表未知
    nnh.param<double>("matchingcostma_radius", matchingcostma_radius_, 0.1);//搜索半径
    nnh.param<double>("matchingcostma_threshold", matchingcostma_threshold_, 0.8);//比例阀值，超过这个比例认为雷达匹配的挺好
    ////true 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。false 可以尝试在宽阔且雷达打不到墙的环境中使用，需要满足一部分雷达点能打到墙壁。
    nnh.param<bool>("is_use_scan_points_map_size", is_use_scan_points_map_size_, false);
    //-------------------------------------------------------------------------------------------------------------------------------------------------------------



    // 创建tf监听器map_points_name_
    tf_listener_new_ = new tf::TransformListener;

    tf_broadcaster_new_= new tf::TransformBroadcaster;





//点的阀值，点云聚类，分割出各个反光柱。至少连续三个点强度超过   强度阀值   即默认为反光柱,单独点滤除
    param_config_.points_size_threshold=points_size_threshold_;
//5725行 indices > 2至少要找到3个点，2个点的话才一条直线匹配错误高广度优先特征匹配
    param_config_.findPolygonIndices_threshold=findPolygonIndices_threshold_;
    //【过滤掉反光柱两俩之间距离太近（在 filter_tow_ont_distance_threshold_ 阀值内就剔除）的点（有可能是数据波动产生的）】
    param_config_.filter_tow_ont_distance_threshold=filter_tow_ont_distance_threshold_;

    //todo  参数服务器
    param_config_.map_frame_id=map_frame_id_;
    param_config_.base_frame_id=base_link_frame_id_;
    param_config_.odom_frame_id=odom_frame_id_;
    param_config_.laser_frame_id=laser_frame_id_;
    param_config_.imu_frame_id=imu_frame_id_;

    param_config_.edgesthreshold=edgesthreshold_;//反光板，柱 的特征匹配的边长特征的阀值
    param_config_.anglesthreshold=anglesthreshold_;//反光板，柱 的特征匹配的两边夹角的阀值
    param_config_.isCircular_threshold=isCircular_threshold_;
    param_config_.isLine_threshold=isLine_threshold_;

    param_config_.time_milliseconds=time_milliseconds_;//匹配上的点的更新速度 ms
    param_config_.scan_distances_threshole=scan_distances_threshole_;//超过这个值为无效数据，雷达有效范围，也在预测范围内，单位m

    param_config_.mapTbase_link_pos_threshold=mapTbase_link_pos_threshold_;//3边定位最小二乘 计算得到的 mapTbase_link  位置阈值
    param_config_.mapTbase_link_rot_threshold=mapTbase_link_rot_threshold_;//3边定位最小二乘 计算得到的  角度阈值，例如10度

    //记录是否已经通过3边定位成功了一次，第一次定位成功就设置true
    param_config_.calculateEdge_threshold=calculateEdge_threshold_;

    //三边定位最小二乘法计算得到的mapTbase_link结果不符合阀值条件的设定的目标超时时间
    param_config_.targetTrilaterationTimeOut=targetTrilaterationTimeOut_;

    //超时说明没有订阅到coatmap的数据
    param_config_.matchcostmapwithscanTimeOut=matchcostmapwithscanTimeOut_;



    param_config_.end_scan_index=end_scan_index_;

    param_config_.fangaungbanHeight=fangaungbanHeight_;
    param_config_.fangaungbanWidth=fangaungbanWidth_;
    param_config_.fangaungbanLength=fangaungbanLength_;

    param_config_.fangaungzhuRadius=fangaungzhuRadius_;
    param_config_.fangaungzhuHeight=fangaungzhuHeight_;
    param_config_.fangaungzhuWidth=fangaungzhuWidth_;
    param_config_.fangaungzhuLength=fangaungzhuLength_;


    //如果平均曲率超过阈值，认为是反光柱
    param_config_.curvature_threshold=curvature_threshold_;
    //在阀值距离范围内，判断 curvature_threshold_ 是反光柱还是反光板
    param_config_.curvature_range_min_threshold=curvature_range_min_threshold_;//距离小于阀值有效 1.5
    param_config_.curvature_range_max_threshold=curvature_range_max_threshold_;//距离大于阀值有效 2.6


    //匹配上的反光板颜色
    param_config_.a=a_;//透明度
    param_config_.r=r_;
    param_config_.g=g_;
    param_config_.b=b_;


    param_config_.scan_intensitie=scan_intensitie_;
    param_config_.intensitie_threshole=intensitie_threshole_;
    param_config_.range_threshole=range_threshole_;
    param_config_.matchok_timeout=matchok_timeout_;
    param_config_.isShowFanGuanBanmarkerPath=isShowFanGuanBanmarkerPath_;//是否显示反光板与小车之间的匹配连线

    param_config_.isSendTf=isSendTf_;
    param_config_.isFirtFanguangbanShowAllMaker=isFirtFanguangbanShowAllMaker_;
    param_config_.is_trilateration=is_trilateration_;
    param_config_.isSend_publish_circle_pointCloud=isSend_publish_circle_pointCloud_;
    param_config_.isStartAreAnglesEqualNew=isStartAreAnglesEqualNew_;
    param_config_.isShowFanGuanBanPoint=isShowFanGuanBanPoint_;
    param_config_.isShowMatchokReflectorFanGuanBanPoint=isShowMatchokReflectorFanGuanBanPoint_;
    //true：当有匹配上反光板后，优先使用匹配好的反光板数据。false：不使用匹配好的反光板数据，一直只使用反光板特征匹配算法
    param_config_.isReadMatchokReflectorPanel=isReadMatchokReflectorPanel_;
    param_config_.isShowMatchokReflectorFanGuanBanPoint=isShowMatchokReflectorFanGuanBanPoint_;
    param_config_.epsilon=epsilon_;
    param_config_.time_log_fig=time_log_fig_;
    param_config_.test_num=test_num_;// 参数服务测试使用，没有实际意思。通过这个参数可以动态的调试代码，把加入到代码调试位置

    //是否配合第三方定位算法一起使用，比如 amcl等。true：表示一起使yong
    param_config_.isUsingThirdPartyLocalization=isUsingThirdPartyLocalization_;


    // 需要分优先顺序，环境中反光板数据上千，上万个选择 isFindReflectorPanelPoints_vector_first_： true，反光板数据上百个isFindReflectorPanelPoints_vector_first_：值随意，最佳选择 isFindReflectorPanelPoints_vector_first_： false。
    param_config_.isFindReflectorPanelPoints_vector_first=isFindReflectorPanelPoints_vector_first_;
    ////当通过key在map中查不到数据时是否遍历列表集合来查询数据,配合 reflectorPanelPoints_vector_size_ 使用代表需要遍历前面多少数据，reflectorPanelPoints_vector_集合中的数据是安预测可能性排序的，可能性越大排序越靠前
    param_config_.isFindReflectorPanelPoints_vector=isFindReflectorPanelPoints_vector_;
    ////是匹配时否搜索 reflectorPanelPointAllUnordered_maps中 的所有数据
    param_config_.isFindRlectorPanelPointAllUnordered_maps=isFindRlectorPanelPointAllUnordered_maps_;
    ////具体评估雷达可见范围内可预测多好数据，reflectorPanelPoints_vector
    param_config_.reflectorPanelPoints_vector_size=reflectorPanelPoints_vector_size_;

    //记录是否已经通过3边定位成功了一次，第一次定位成功就设置true
    param_config_.firstTrilateration_with_yawFig=firstTrilateration_with_yawFig_;
    //角度方差 定义一个方差阀值,判断当前通过3边定位算法出来的结果与里程计的方差是否小于阀值
    param_config_.variance_yaw_threshold=variance_yaw_threshold_;
    //满足角度调整阀值，可以设置 variance_yaw_threshold_ 的倍数作为参数
    param_config_.variance_adjustment_yaw_threshold=variance_adjustment_yaw_threshold_;

    //位置方差 定义一个方差阀值,判断当前通过3边定位算法出来的结果与里程计的方差是否小于阀值
    param_config_.variance_pose_threshold=variance_pose_threshold_;
    //满足位置调整阀值，可以设置 variance_pose_threshold_ 的倍数作为参数
    param_config_.variance_adjustment_pose_threshold=variance_adjustment_pose_threshold_;


    //位置方差 超过这个精度就使用反光柱定位数据进行定位纠正
    param_config_.variance_min_pose_lerance=variance_min_pose_lerance_;
    //角度方差 超过这个精度就使用反光柱定位数据进行定位纠正
    param_config_.variance_min_yaw_lerance=variance_min_yaw_lerance_;


    //匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿,如果score 大于阀值 1.2且小于阀值  等于3.0，表示匹配成功的。 ComputeCurrentPose 10090
    param_config_.score_min_threshold=score_min_threshold_;
    //匹配上的当前反光柱与全局反光柱的集合，返回出一个大致的机器人位姿,如果score 大于阀值 1.2且小于阀值  等于3.0，表示匹配成功的。 ComputeCurrentPose 10090
    param_config_.score_max_threshold=score_max_threshold_;


    //连续出现3次定位失败就重新置为 firstTrilateration_with_yawFig_ = fasle。重新定位
    param_config_.isNotMin_varianceNum_target=isNotMin_varianceNum_target_;


    //是否启动判断是否是原地旋转功能，启动后 原地旋转 不会更新定位数据
    param_config_.is_start_place_rotation=is_start_place_rotation_;

    //is_start_place_rotation=true 后会加载 rotate_in_place_variance_multiplier 在原地旋转时放大定位精度，从而减少定位的抖动带来的移动机器人反复调整位置.
    param_config_.rotate_in_place_variance_multiplier=rotate_in_place_variance_multiplier_;

    //-----------------------------fanguangbantrilateration/src/test/test52.cpp--------------------------
    //match_costmap_with_scan_service 验证移动机器人位置以及雷达点云匹配律是否可靠，从另一类 fanguangbantrilateration/src/test/test52.cpp
    param_config_.isPublish_matchcostmapwithscan_mssage=isPublish_matchcostmapwithscan_mssage_;
    param_config_.free_cell=free_cell_;
    param_config_.occupied_cell=occupied_cell_;
    param_config_.unknown_cell=unknown_cell_;
    param_config_.matchingcostma_radius=matchingcostma_radius_;
    param_config_.matchingcostma_threshold=matchingcostma_threshold_;
    param_config_.is_use_scan_points_map_size=is_use_scan_points_map_size_;

    //------------------------------------------------------------------



    //中位数滤波器 窗口大小
    param_config_.window_size=window_size_;
    //低通滤波器 alpha 值为 0.1
    param_config_.alpha=alpha_;

    param_config_.update_point_id="EMPTY";
    param_config_.update_point_x=0.0;
    param_config_.update_point_y=0.0;

    param_config_.update_factor=0.01;

    //参数服务器
    dynamic_reconfigure::Server<fanguangbantrilateration::TrilaterationParamConfig> server;
    dynamic_reconfigure::Server<fanguangbantrilateration::TrilaterationParamConfig>::CallbackType f;

    f = boost::bind(&dynamic_reconfigure_callback, _1, _2);
    server.setCallback(f);



    {

        iswirtortSetPeriodically_=true;
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        //读取雷达对应的强度数据，存入std::map<int,double> &scan_intensitie_map  ,  scan_intensitie_key_
        loadScanIntensitiesJSONDataFromFile( scanIntensitiesFilePath_,scan_intensitie_map_);

        //获取的JSON中的反光板单个点位
        loadAllPointJSONDataFromFile( savePointFilePath_,reflectorPanelPoints_vector_,reflectorPanelPointAllMaps_,reflectorPanelPointAllUnordered_maps_);
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        iswirtortSetPeriodically_=false;

        //}//if(false)  2.3

    }

    std::cout<<"1.==================================启动处理队列的线程============================="<<std::endl;
    // 启动处理队列的线程，处理已经匹配完成的反光板数据，发布tf数据
    std::thread processing_thread(processQueue);//发布计算坐标并发布tf数据
    std::thread updatefun_thread(updatefun);
    std::thread verificationThread(check_atch_costmap_with_scan_nodeStatusfun);//心跳，检查校验节点是否启动
    std::cout<<"2.==================================启动处理队列的线程============================="<<std::endl;







    //发布性能评估数据
    time_log_pub_ = nh.advertise<fanguangbantrilateration::trilateration_read_time_log>(time_log_topic_name_, 10);

    // 创建 service 服务 request_handlers_ 函数映射表
    request_handlers_ = createRequestHandlers();
    // service 服务 /trilateration_service
    ros::ServiceServer service = nh.advertiseService(service_name_, trilaterationServiceCallback);

    // 记录 发布重定位 开始时间
    startTime_ = std::chrono::high_resolution_clock::now();
    //经典三边定位最小二乘法 每次计算得到的 mapTbase_link 的开始时间
    trilateration_mapTbase_link_pose_startTime_ = std::chrono::high_resolution_clock::now();

    //判断是否有数据过来，超过时间没有数据认为，订阅不成功
    resultGetResultMapvalueCallback_startTime_ = std::chrono::high_resolution_clock::now();
    resultGetCalculateScanMapRatioCallback_startTime_ = std::chrono::high_resolution_clock::now();

    // 记录开始时间
    // auto start = std::chrono::high_resolution_clock::now();

    //  /scan
    ros::Subscriber  scan_sub = nh.subscribe<sensor_msgs::LaserScan>(scan_toptic_name_, 100, boost::bind(&scanCallback, _1));

    // 订阅里程计数据
    ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>("odom", 100, odomCallback);


    // "/trilateration_marker"
    pub_marker_ = nh.advertise<visualization_msgs::Marker>(show_marker_toptic_name_, 10);
    //  "/trilateration_all_marker"
    pub_all_marker_ = nh.advertise<visualization_msgs::Marker>(show_all_marker_toptic_name_, 10, true);
    //todo
    pub_all_current_marker_= nh.advertise<visualization_msgs::Marker>(show_all_current_marker_toptic_name_, 10, true);
    // "/trilateration_circle_points"
    pub_circle_pointCloud_ = nh.advertise<sensor_msgs::PointCloud2>(show_circle_points_toptic_name_, 1);

    // 订阅 /clicked_point 话题，并指定回调函数，取地图反光板点位
    ros::Subscriber sub = nh.subscribe(get_points_toptic_name_, 10, clickedPointCallback);

    // "/initialpose" 其实调用了handleInitialPoseMessage，处理初始化位姿
    ros::Subscriber initial_pose_sub = nh.subscribe(initialpose_toptic_name_, 2, &initialPoseReceived);

    //重定位
    initial_pose_pub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(initialpose_toptic_name_, 10);

    //==================================================================================


    pub_point1_ = nh.advertise<geometry_msgs::PointStamped>("/pointStamped1", 1);
    pub_point2_ = nh.advertise<geometry_msgs::PointStamped>("/pointStamped2", 1);
    pub_point3_ = nh.advertise<geometry_msgs::PointStamped>("/pointStamped3", 1);
    pub_point4_ = nh.advertise<geometry_msgs::PointStamped>("/pointStamped4", 1);
    pub_point5_ = nh.advertise<geometry_msgs::PointStamped>("/pointStamped5", 1);


    //=================================================================================

    // 开启一个线程定时对集合进行排序,从小到大排序 1651
    /**
       std::ref是C++标准库中的一个函数，它用于创建一个引用包装器（std::reference_wrapper），
     这个包装器封装了一个引用，并可以被用作可调用对象的参数。当你传递一个非引用类型的对象给线程函数（如std::thread的构造函数）时，
     对象会被复制。然而，有些情况下，你可能希望线程函数直接操作原始对象而不是其副本，这时就需要使用std::ref。
     */
    // std::thread sortThread(sortSetPeriodically, std::ref(reflectorPanelPoints_vector_), 1000); // 每隔 1 秒排序reflectorPanelPoints_vector_一次，距离从小到大

    //=========================================================================



    //订阅costmap数据
    ros::Subscriber resultGetResultMapvalue_sub = nh.subscribe<fanguangbantrilateration::matchcostmapwithscan_result>(getResultMapvalue_result_toptic_name_, 10, resultGetResultMapvalueCallback);
    ros::Subscriber getCalculateScanMapRatio_sub = nh.subscribe<fanguangbantrilateration::matchcostmapwithscan_result>(getCalculateScanMapRatio_result_toptic_name_, 10, resultGetCalculateScanMapRatioCallback);
    // 创建一个服务客户端，指定服务名称和服务消息类型
    match_costmap_with_scan_service_client_ = nh.serviceClient<fanguangbantrilateration::matchcostmapwithscan_srvs>(match_costmap_with_scan_service_name_);


    //==================================================================
    //初始化
    {
        //isUsingThirdPartyLocalization_ 是否配合第三方定位算法一起使用，比如 amcl等。true：表示一起使yong
        if(isUsingThirdPartyLocalization_){
            static tf::TransformListener tf_listener;
            tf::StampedTransform odom_to_map_transform;//mapTodom

            try {

                tf_listener.waitForTransform(map_frame_id_, odom_frame_id_,ros::Time(0), ros::Duration(2.0));
                tf_listener.lookupTransform(map_frame_id_, odom_frame_id_, ros::Time(0), odom_to_map_transform);
                std::this_thread::sleep_for(std::chrono::milliseconds(5));
                tf_listener.waitForTransform(base_link_frame_id_, laser_frame_id_,ros::Time(0), ros::Duration(2.0));
                tf_listener.lookupTransform(base_link_frame_id_, laser_frame_id_, ros::Time(0), laser_to_base_link_transform_);


            } catch (tf::TransformException& ex) {
                tf_last_odom_to_map_transform_ = tf::Transform::getIdentity();//初始化数据
                std::cout <<"订阅 odom_to_map  发布 tf " << std::endl;
                ROS_WARN("%s", ex.what());

            }
        }else{
            tf_last_odom_to_map_transform_ = tf::Transform::getIdentity();//初始化数据
        }

    }





    //----------------------------------------------------------------------------------------------------------------
    {

        //计算好的特征文件的逻辑
        loadCalculateEdgeJSONDataFromFile(calculateFilePath_, reflectorPanelPointAllMaps_, unorderedEdgeMapNew_,
                                          unorderedEdgeMapNew_IdKey_);

        std::this_thread::sleep_for(std::chrono::milliseconds(500));//todo 不延时会加载不到数据
        //显示所有的反光柱
        fanguangbanShowAllMaker(reflectorPanelPointAllMaps_);

    }


    std::cout<<"calculateFilePath_:"<<calculateFilePath_<<std::endl;
    std::cout<<"unorderedEdgeMapNew_.size():"<<unorderedEdgeMapNew_.size()<<std::endl;
    std::cout<<"reflectorPanelPointAllMaps_.size():"<<reflectorPanelPointAllMaps_.size()<<std::endl;
    std::cout<<"reflectorPanelPoints_vector_.size():"<<reflectorPanelPoints_vector_.size()<<std::endl;
    std::cout<<"ReflectorPanelPoint::instances.size():"<<ReflectorPanelPoint::instances.size()<<std::endl;



    //----------------------------------------------------------------------------------------------------------------



    //  }//if(false)   2.3



    std::thread updatefunFanguangbanShowAllMaker_thread(updatefunFanguangbanShowAllMaker);


    ros::spin();

    // 关闭线程
    {

//        std::lock_guard<std::mutex> lock(queue_mutex_);
//        std::unique_lock<std::mutex> lock1(sortSetPeriodically_mutex_);
//        stop_thread_ = true;
    }
    queue_cond_var_.notify_one();    // 加锁

    if (processing_thread.joinable()) {
        processing_thread.join();
    }
    if (updatefun_thread.joinable()) {
        updatefun_thread.join();
    }

    if (verificationThread.joinable()) {
        verificationThread.join();
    }
    if (updatefunFanguangbanShowAllMaker_thread.joinable()) {
        updatefunFanguangbanShowAllMaker_thread.join();
    }




    //删除所有容器数据
    deleteAllContainerData();//存在了 sortSetPeriodically_cond_var_.notify_one();

    // 释放tf监听器
    delete tf_listener_new_;
    delete tf_broadcaster_new_;

    // 等待排序线程结束
    //  sortSetPeriodically_cond_var_.notify_one();
//    if (sortThread.joinable()) {
//        sortThread.join();
//    }

    return 0;
}




//查找请求类型对应的处理函数,默认使用函数名即为 request_handlers_ 中的 key
void processRequest(const std::string& request_type, const fanguangbantrilateration::trilateration_srvs::Request& req, fanguangbantrilateration::trilateration_srvs::Response& res) {
    // 查找请求类型对应的处理函数, 默认使用函数名即为 request_handlers_ 中的 key
    auto it = request_handlers_.find(request_type);
    if (it != request_handlers_.end()) {
        // 调用找到的处理函数并传递请求和响应对象
        it->second(req, res);
    } else {
        // 输出无效的请求类型错误信息
        std::cerr << "Invalid request type: " << request_type << std::endl;
        res.resultTxt="Invalid request type ";
        res.result="error";
        res.success=false;
    }
}

//接收用户指令的服务,"/trilateration_service"
bool trilaterationServiceCallback(fanguangbantrilateration::trilateration_srvs::Request &req,
                                  fanguangbantrilateration::trilateration_srvs::Response &res)
{
    // 自锁，确保同一时间只有一个线程访问此部分代码
    std::lock_guard<std::mutex> lock(callback_mutex);

    // 调用 processRequest 函数处理请求和响应， request_type 默认使用函数名即为 request_handlers_ 中的 key
    processRequest(req.request_type, req, res);
    return true; // Return true to indicate success
}
























































